#include "NavierStokesSolver.h"
#include "RenderMiscs.h"
#include <set>
#include <queue>
#ifdef _DENSITY
#include "densityField.h"
#endif
#ifdef _PARTICLES
#include "ParticleSys.h"
#endif
#ifdef _SLC
#include "slcSurface.H"
#else
//#include "ParticleLevelSet.h"
#endif
#include "DecoLogger.h"
#include "ConfigManager.h"
#include "AccurateTwoWayCoupler.h"
#include <utility>
using namespace std;

#define FAST_SWEEP 0
INT detailDepth = -2;
static INT marchingGrids = 4;
const BOOL bUsePressureBoundary = TRUE;
static const BOOL bRandomHack = FALSE;
#ifdef _SLC
static const BOOL bUseSurfacePressure = TRUE;
INT bRemoveBottom = 1;
#else
static const BOOL bUseSurfacePressure = FALSE;
#endif
//static const INT ResSmoke = 2;



//DWORD g_dwTlsNSSolver = TLS_OUT_OF_INDEXES;
//
//
//
//NSSolver* NSSolver::GetSingleton()
//{
//	if (g_dwTlsNSSolver == TLS_OUT_OF_INDEXES)
//
//	{
//
//		g_dwTlsNSSolver = TlsAlloc();
//
//		TlsSetValue(g_dwTlsNSSolver, NULL);
//
//	}
//
//
//
//	NSSolver* p = (NSSolver*)TlsGetValue(g_dwTlsNSSolver);
//
//	if (p)
//
//		return p;
//
//	else
//
//	{
//
//		p = new NSSolver();
//
//		TlsSetValue(g_dwTlsNSSolver, p);
//
//		return p;
//
//	}
//}
//void NSSolver::DestroySingleton()
//{
//	if (g_dwTlsNSSolver == TLS_OUT_OF_INDEXES)
//		return;
//	else
//	{
//		NSSolver *pDM = (NSSolver*)TlsGetValue(g_dwTlsNSSolver);
//		if (pDM != NULL)
//		{
//			delete pDM;
//			TlsSetValue(g_dwTlsNSSolver, NULL);
//		}
//	}
//}

NSSolver::NSSolver() :	
	m_viscocity(unifiedViscosity), 
	m_force(vector3(0, -9.8, 0)),
	m_tension(0.0732),
	m_blockSizeX(blockXWidth),
	m_blockSizeY(blockYWidth),
	m_blockSizeZ(blockZWidth),
	m_showLayer(1),
	m_grid(NULL),
	divergenceVelocity(NULL),
	ADiag(NULL),
	APlusi(NULL),
	APlusj(NULL),
	APlusk(NULL),
	tmpDomain(NULL),
	//p(NULL),
	s(NULL),
	z(NULL),
	precon(NULL),
	r(NULL),
	q(NULL),
	tmpU(NULL),
	tmpV(NULL),
	tmpW(NULL),
	mbMatrixFormed(false),
	mCoupler(NULL),
	m_scene(NULL)
{
	DecoConfig::GetSingleton()->GetInt("Solver", "marchingGrids", marchingGrids);
}


void NSSolver::fillAllGridWithFluid(INT airMargin, bool bRefillParticles)
{
	static bool bNeedFillParticle = true;
	fillAllGridWithFluid(m_grid, airMargin);
	if (bNeedFillParticle || bRefillParticles)
		m_scene->particles.FillParticles();
	bNeedFillParticle = false;
}
void NSSolver::fillAllGridWithFluid(GridField* grid, INT airMargin)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;

	CopyTo3DArray(grid->prevInDomain, grid->inDomain, numX, numY, numZ);
	MemSet3DArray(grid->inDomain, 0, numX, numY, numZ);
	grid->m_compDomain.clear();
	grid->m_validU.clear();
	grid->m_validV.clear();
	grid->m_validW.clear();
	for (INT i = airMargin; i < numX - airMargin; i++)
	{
		for (INT j = airMargin; j < numY - airMargin; j++)
		{
			for (INT k = airMargin; k < numZ - airMargin; k++)
			{
				if (grid->type[i][j][k] != GT_Solid && grid->type[i][j][k] != GT_Sprayer)
				{
					grid->type[i][j][k] = GT_FLuid;
					grid->fastType[i+1][j+1][k+1] = GT_FLuid;
					grid->m_compDomain.push_back(Index3(i, j, k));
					grid->inDomain[i][j][k] = TRUE;
				}
			}
		}
	}

	INT subGridCount = grid->volxelPrec * grid->volxelPrec * grid->volxelPrec;
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				INT count = 0;
				for (INT x = 0; x < grid->volxelPrec; x++)
				{
					for (INT y = 0; y < grid->volxelPrec; y++)
					{
						for (INT z = 0; z < grid->volxelPrec; z++)
						{
							if(solidElements[i * grid->volxelPrec + x][j * grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}

				grid->cFluidVol[i][j][k] = count / FLOAT(subGridCount);
// 				if (grid->type[i][j][k] != GT_Solid)
// 				{
// 					INT count = 0;
// 					for (INT x = 0; x < grid->volxelPrec; x++)
// 					{
// 						for (INT y = 0; y < grid->volxelPrec; y++)
// 						{
// 							for (INT z = 0; z < grid->volxelPrec; z++)
// 							{
// 								if(solidElements[i * grid->volxelPrec + x][j * grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
// 									count++;
// 							}
// 						}
// 					}
// 
// 					grid->cFluidVol[i][j][k] = count / FLOAT(subGridCount);
// 				}
// 				else
// 					grid->cFluidVol[i][j][k] = 0.0f;
			}
		}
	}
	for (INT i = 1; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				INT count = 0;
				for (INT x = grid->volxelPrec / 2; x < grid->volxelPrec; x++)
				{
					for (INT y = 0; y < grid->volxelPrec; y++)
					{
						for (INT z = 0; z < grid->volxelPrec; z++)
						{
							if(solidElements[(i - 1) * grid->volxelPrec + x][j * grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}
				for (INT x = 0; x < grid->volxelPrec / 2; x++)
				{
					for (INT y = 0; y < grid->volxelPrec; y++)
					{
						for (INT z = 0; z < grid->volxelPrec; z++)
						{
							if(solidElements[i * grid->volxelPrec + x][j * grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}

// 				grid->uFluidVol[i][j][k] = count / FLOAT(subGridCount);
// 				if(grid->uFluidVol[i][j][k] > 0.0f)
// 					grid->m_validU.push_back(Index3(i, j, k));

				if (grid->type[i][j][k] != GT_Solid && grid->type[i - 1][j][k] != GT_Solid)
				{
					grid->m_validU.push_back(Index3(i, j, k));
					grid->uFluidVol[i][j][k] = count / FLOAT(subGridCount);
				}
				else if (grid->type[i][j][k] != GT_Solid && grid->type[i - 1][j][k] == GT_Solid
					|| grid->type[i][j][k] == GT_Solid && grid->type[i - 1][j][k] != GT_Solid)
					grid->uFluidVol[i][j][k] = count / FLOAT(subGridCount);
 				else
					grid->uFluidVol[i][j][k] = 0.0f;
			}
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 1; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				INT count = 0;
				for (INT x = 0; x < grid->volxelPrec; x++)
				{
					for (INT y = grid->volxelPrec / 2; y < grid->volxelPrec; y++)
					{
						for (INT z = 0; z < grid->volxelPrec; z++)
						{
							if(solidElements[i * grid->volxelPrec + x][(j - 1)* grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}
				for (INT x = 0; x < grid->volxelPrec; x++)
				{
					for (INT y = 0; y < grid->volxelPrec / 2; y++)
					{
						for (INT z = 0; z < grid->volxelPrec; z++)
						{
							if(solidElements[i * grid->volxelPrec + x][j * grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}

// 				grid->vFluidVol[i][j][k] = count / FLOAT(subGridCount);
// 				if(grid->vFluidVol[i][j][k] > 0.0f)
// 					grid->m_validV.push_back(Index3(i, j, k));

				if (grid->type[i][j][k] != GT_Solid && grid->type[i][j - 1][k] != GT_Solid)
				{
					grid->m_validV.push_back(Index3(i, j, k));
					grid->vFluidVol[i][j][k] = count / FLOAT(subGridCount);
				}
				else if (grid->type[i][j][k] != GT_Solid && grid->type[i][j - 1][k] == GT_Solid
					|| grid->type[i][j][k] == GT_Solid && grid->type[i][j - 1][k] != GT_Solid)
					grid->vFluidVol[i][j][k] = count / FLOAT(subGridCount);
 				else
					grid->vFluidVol[i][j][k] = 0.0f;
			}
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 1; k < numZ; k++)
			{
				INT count = 0;
				for (INT x = 0; x < grid->volxelPrec; x++)
				{
					for (INT y = 0; y < grid->volxelPrec; y++)
					{
						for (INT z = grid->volxelPrec / 2; z < grid->volxelPrec; z++)
						{
							if(solidElements[i * grid->volxelPrec + x][j * grid->volxelPrec + y][(k - 1) * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}
				for (INT x = 0; x < grid->volxelPrec; x++)
				{
					for (INT y = 0; y < grid->volxelPrec; y++)
					{
						for (INT z = 0; z < grid->volxelPrec / 2; z++)
						{
							if(solidElements[i * grid->volxelPrec + x][j * grid->volxelPrec + y][k * grid->volxelPrec + z] == 0)
								count++;
						}
					}
				}

// 				grid->wFluidVol[i][j][k] = count / FLOAT(subGridCount);
// 				if(grid->wFluidVol[i][j][k] > 0.0f)
// 					grid->m_validW.push_back(Index3(i, j, k));
 
				if (grid->type[i][j][k] != GT_Solid && grid->type[i][j][k - 1] != GT_Solid)
				{
					grid->m_validW.push_back(Index3(i, j, k));
					grid->wFluidVol[i][j][k] = count / FLOAT(subGridCount);
				}
				else if (grid->type[i][j][k] != GT_Solid && grid->type[i][j][k - 1] == GT_Solid
					|| grid->type[i][j][k] == GT_Solid && grid->type[i][j][k - 1] != GT_Solid)
					grid->wFluidVol[i][j][k] = count / FLOAT(subGridCount);
 				else
					grid->wFluidVol[i][j][k] = 0.0f;
			}
		}
	}
}


void NSSolver::addForce(GridField* grid)
{

	DOUBLE TIME_STEP = m_scene->GetTimeStep();
	INT validUSize = static_cast<INT>(grid->m_validU.size());
	for (INT it = 0; it < validUSize; it++)
	{
		INT i = grid->m_validU[it].m_i;
		INT j = grid->m_validU[it].m_j;
		INT k = grid->m_validU[it].m_k;
		grid->u[i][j][k] += TIME_STEP * m_force.x;
	}
	INT validVSize = static_cast<INT>(grid->m_validV.size());
	for (INT it = 0; it < validVSize; it++)
	{
		INT i = grid->m_validV[it].m_i;
		INT j = grid->m_validV[it].m_j;
		INT k = grid->m_validV[it].m_k;
		grid->v[i][j][k] += TIME_STEP * m_force.y;
	}
	INT validWSize = static_cast<INT>(grid->m_validW.size());
	for (INT it = 0; it < validWSize; it++)
	{
		INT i = grid->m_validW[it].m_i;
		INT j = grid->m_validW[it].m_j;
		INT k = grid->m_validW[it].m_k;
		grid->w[i][j][k] += TIME_STEP * m_force.z;
	}

}

void NSSolver::addForce()
{		
	addForce(m_grid);

}

vector3 NSSolver::semiLagrange(const GridField& valueGrid, const vector3& pos, const vector3& velocity, DOUBLE timeStep)
{
	vector3 newpos = pos - timeStep * velocity;
	return valueGrid.SampleVelocity(newpos);
}

void NSSolver::convectWithoutBFECC(const GridField& velocityField, const GridField& valueField, GridField& resultField, 
								   const vector<Index3>& indicesU, const vector<Index3>& indicesV, const vector<Index3>& indicesW)
{
	assert(&valueField != &resultField);

	DOUBLE TIME_STEP = m_scene->GetTimeStep();
	INT validUSize = static_cast<INT>(indicesU.size());
	for (INT it = 0; it < validUSize; it++)
	{
		INT i = indicesU[it].m_i;
		INT j = indicesU[it].m_j;
		INT k = indicesU[it].m_k;

		vector3 pos = valueField.GetGridCenter(i, j, k) - vector3(valueField.stepSize * 0.5, 0, 0);
		vector3 vel = velocityField.SampleVelocity(pos, m_grid);
		vector3 newpos = pos - TIME_STEP * vel;
		resultField.u[i][j][k] = valueField.SampleU(newpos, m_grid);
	}
	INT validVSize = static_cast<INT>(indicesV.size());
	for (INT it = 0; it < validVSize; it++)
	{
		INT i = indicesV[it].m_i;
		INT j = indicesV[it].m_j;
		INT k = indicesV[it].m_k;

		
		vector3 pos = valueField.GetGridCenter(i, j, k) - vector3(0, valueField.stepSize * 0.5, 0);
		vector3 vel = velocityField.SampleVelocity(pos, m_grid);
		vector3 newpos = pos - TIME_STEP * vel;
		resultField.v[i][j][k] = valueField.SampleV(newpos, m_grid);
	}
	INT validWSize = static_cast<INT>(indicesW.size());
	for (INT it = 0; it < validWSize; it++)
	{
		INT i = indicesW[it].m_i;
		INT j = indicesW[it].m_j;
		INT k = indicesW[it].m_k;

		vector3 pos = valueField.GetGridCenter(i, j, k) - vector3(0, 0, valueField.stepSize * 0.5);
		vector3 vel = velocityField.SampleVelocity(pos, m_grid);
		vector3 newpos = pos - TIME_STEP * vel;
		resultField.w[i][j][k] = valueField.SampleW(newpos, m_grid);
	}

}

void NSSolver::convection(GridField* grid) 
{
	static BOOL bBFECC = TRUE;

	if (!bBFECC)
	{
#ifdef _DEBUG_TIME
		DecoTimer t;
		t.startTimer();
#endif
		UINT numX = grid->numXElement;
		UINT numY = grid->numYElement;
		UINT numZ = grid->numZElement;

#ifdef _DEBUG_TIME
		DOUBLE time1 = t.getCurrentTime();
		t.resetTimer();
#endif

		

#ifdef _DEBUG_TIME
		DOUBLE time2 = t.getCurrentTime();
		t.resetTimer();
#endif
		convectWithoutBFECC(*grid, *grid, resultField, grid->m_validU, grid->m_validV, grid->m_validW);
		INT validUSize = static_cast<INT>(grid->m_validU.size());
		for (INT it = 0; it < validUSize; it++)
		{
			INT i = grid->m_validU[it].m_i;
			INT j = grid->m_validU[it].m_j;
			INT k = grid->m_validU[it].m_k;
			grid->u[i][j][k] = resultField.u[i][j][k];
		}
		INT validVSize = static_cast<INT>(grid->m_validV.size());
		for (INT it = 0; it < validVSize; it++)
		{
			INT i = grid->m_validV[it].m_i;
			INT j = grid->m_validV[it].m_j;
			INT k = grid->m_validV[it].m_k;
			grid->v[i][j][k] = resultField.v[i][j][k];
		}
		INT validWSize = static_cast<INT>(grid->m_validW.size());
		for (INT it = 0; it < validWSize; it++)
		{
			INT i = grid->m_validW[it].m_i;
			INT j = grid->m_validW[it].m_j;
			INT k = grid->m_validW[it].m_k;
			grid->w[i][j][k] = resultField.w[i][j][k];
		}
		//CopyTo3DArray(grid->u, resultField.u, numX + 1, numY, numZ);
		//CopyTo3DArray(grid->v, resultField.v, numX, numY + 1, numZ);
		//CopyTo3DArray(grid->w, resultField.w, numX, numY, numZ + 1);
#ifdef _DEBUG_TIME
		DOUBLE time3 = t.getCurrentTime();
		//(*DecoLogger::GetSingleton()) << "copy constructor spent " << time1 << " seconds\n";
		//(*DecoLogger::GetSingleton()) << "convectWithoutBFECC spent " << time2 << " seconds\n";
		//(*DecoLogger::GetSingleton()) << "copyTo3DArray spent " << time3 << " seconds\n";
#endif
	}
	else
	{
		UINT numX = grid->numXElement;
		UINT numY = grid->numYElement;
		UINT numZ = grid->numZElement;
		//GridField resultField;
		resultField.CopyVelocityFrom(*grid, numX, numY, numZ);
		convectWithoutBFECC(*grid, *grid, resultField, grid->m_validU, grid->m_validV, grid->m_validW);

		tmpMinusVelField.CopyVelocityFrom(*grid, numX, numY, numZ);
		for (UINT i = 0; i < numX + 1; i++)
			for (UINT j = 0; j < numY; j++)
				for (UINT k = 0; k < numZ; k++)
					tmpMinusVelField.u[i][j][k] = -tmpMinusVelField.u[i][j][k];
		for (UINT i = 0; i < numX; i++)
			for (UINT j = 0; j < numY + 1; j++)
				for (UINT k = 0; k < numZ; k++)
					tmpMinusVelField.v[i][j][k] = -tmpMinusVelField.v[i][j][k];
		for (UINT i = 0; i < numX; i++)
			for (UINT j = 0; j < numY; j++)
				for (UINT k = 0; k < numZ + 1; k++)
					tmpMinusVelField.w[i][j][k] = -tmpMinusVelField.w[i][j][k];

		//GridField errorField;
		errorField.CopyVelocityFrom(*grid, numX, numY, numZ);
		convectWithoutBFECC(tmpMinusVelField, resultField, errorField, grid->m_validU, grid->m_validV, grid->m_validW);

		for (UINT i = 0; i < numX + 1; i++)
			for (UINT j = 0; j < numY; j++)
				for (UINT k = 0; k < numZ; k++)
					errorField.u[i][j][k] = grid->u[i][j][k] + 0.5 * (grid->u[i][j][k]-errorField.u[i][j][k]);
		for (UINT i = 0; i < numX; i++)
			for (UINT j = 0; j < numY + 1; j++)
				for (UINT k = 0; k < numZ; k++)
					errorField.v[i][j][k] = grid->v[i][j][k] + 0.5 * (grid->v[i][j][k]-errorField.v[i][j][k]);
		for (UINT i = 0; i < numX; i++)
			for (UINT j = 0; j < numY; j++)
				for (UINT k = 0; k < numZ + 1; k++)
					errorField.w[i][j][k] = grid->w[i][j][k] + 0.5 * (grid->w[i][j][k]-errorField.w[i][j][k]);

		//GridField resultField2;
		resultField2.CopyVelocityFrom(resultField, numX, numY, numZ);

		convectWithoutBFECC(*grid, errorField, resultField2, grid->m_validU, grid->m_validV, grid->m_validW);
		CopyTo3DArray(grid->u, resultField2.u, numX + 1, numY, numZ);
		CopyTo3DArray(grid->v, resultField2.v, numX, numY + 1, numZ);
		CopyTo3DArray(grid->w, resultField2.w, numX, numY, numZ + 1);
		INT indiceSize = static_cast<INT>(grid->m_compDomain.size());
		for (INT it = 0; it < indiceSize; it++)
		{
			INT i = grid->m_compDomain[it].m_i;
			INT j = grid->m_compDomain[it].m_j;
			INT k = grid->m_compDomain[it].m_k;
			if (grid->isBoundaryCell(i, j, k))
			{
				grid->u[i][j][k] = resultField.u[i][j][k];
				grid->u[i + 1][j][k] = resultField.u[i + 1][j][k];
				grid->v[i][j][k] = resultField.v[i][j][k];
				grid->v[i][j + 1][k] = resultField.v[i][j + 1][k];
				grid->w[i][j][k] = resultField.w[i][j][k];
				grid->w[i][j][k + 1] = resultField.w[i][j][k + 1];
			}
		}
	}
}
void NSSolver::convection()
{
	convection(m_grid);
}


void NSSolver::projection(GridField* grid, DOUBLE*** divergenceVelocity, DOUBLE*** ADiag, DOUBLE*** APlusi, DOUBLE*** APlusj, DOUBLE*** APlusk, BOOL*** tmpDomain)
{
	UINT numX = grid->numXElement;
	UINT numY = grid->numYElement;
	UINT numZ = grid->numZElement;


	CopyTo3DArray(tmpDomain, grid->inDomain, numX, numY, numZ);
	vector<Index3> indices;

	formMatrixAndDivergence(grid, ADiag, APlusi, APlusj, APlusk, divergenceVelocity, indices, tmpDomain);

	solveForPressure(grid, ADiag, APlusi, APlusj, APlusk, divergenceVelocity, indices, tmpDomain);

	projectVelocity(grid, indices, tmpDomain);

}

void NSSolver::projection()
{

	UINT numX = m_grid->numXElement;
	UINT numY = m_grid->numYElement;
	UINT numZ = m_grid->numZElement;
#ifdef _DEBUG_TIME
	DecoTimer t;
	t.startTimer();
#endif
	Zero3DArray(divergenceVelocity, numX, numY, numZ);
	Zero3DArray(ADiag, numX, numY, numZ);
	Zero3DArray(APlusi, numX, numY, numZ);
	Zero3DArray(APlusj, numX, numY, numZ);
	Zero3DArray(APlusk, numX, numY, numZ);
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);


	projection(m_grid, divergenceVelocity, ADiag, APlusi, APlusj, APlusk, tmpDomain);

}

void NSSolver::extrapolateVelocity()
{	

	initializeLevelSet(m_grid);
	fastMarching(m_grid, marchingGrids * m_grid->stepSize);

}

void NSSolver::enforceSolidVeloctiy(GridField* grid)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;

	vector3 sprayerVelocity(0, 0, 0);
	DecoConfig::GetSingleton()->GetVector3("Sprayer", "velocity", sprayerVelocity);

	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
#ifdef _SLC
				if (grid->isSprayerCell(i, j, k))
				{
					if (sprayerVelocity.x > 0)
					{
						if (!grid->isSprayerCell(i - 1, j, k))
						{
							grid->u[i][j][k] = grid->u[i + 1][j][k] = 0;
						}
						else
						{
							grid->u[i + 1][j][k] = sprayerVelocity.x;
						}
					}
					else 
					{
						if (!grid->isSprayerCell(i + 1, j, k))
						{
							grid->u[i][j][k] = grid->u[i + 1][j][k] = 0;
						}
						else
						{
							grid->u[i][j][k] = sprayerVelocity.x;
						}
					}
					if (sprayerVelocity.y > 0)
					{
						if (!grid->isSprayerCell(i, j - 1, k))
						{
							grid->v[i][j][k] = grid->v[i][j + 1][k] = 0;
						}
						else
						{
							grid->v[i][j + 1][k] = sprayerVelocity.y;
						}
					}
					else
					{
						if (!grid->isSprayerCell(i, j + 1, k))
						{
							grid->v[i][j][k] = grid->v[i][j + 1][k] = 0;
						}
						else
						{
							grid->v[i][j][k] = sprayerVelocity.y;
						}
					}

					if (sprayerVelocity.z > 0)
					{
						if (!grid->isSprayerCell(i, j, k - 1))
						{
							grid->w[i][j][k] = grid->w[i][j][k + 1] = 0;
						}
						else
						{
							grid->w[i][j][k + 1] = sprayerVelocity.z;
						}
					}
					else
					{
						if (!grid->isSprayerCell(i, j, k + 1))
						{
							grid->w[i][j][k] = grid->w[i][j][k + 1] = 0;
						}
						else
						{
							grid->w[i][j][k] = sprayerVelocity.z;
						}
					}
					
				}
#else
				if (grid->isSprayerCell(i, j, k))
				{
					grid->u[i][j][k] = grid->u[i + 1][j][k] = sprayerVelocity.x;
					grid->v[i][j][k] = grid->v[i][j + 1][k] = sprayerVelocity.y;
					grid->w[i][j][k] = grid->w[i][j][k + 1] = sprayerVelocity.z;
			}
#endif
			}

		}
	}
}

void NSSolver::enforceSolidVeloctiy()
{
	enforceSolidVeloctiy(m_grid);

}

void NSSolver::initializeLevelSet(GridField* marchingGrid)
{
	INT numX = marchingGrid->numXElement;
	INT numY = marchingGrid->numYElement;
	INT numZ = marchingGrid->numZElement;
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
#ifndef _PLS
				if (marchingGrid->isSurfaceCell(i, j, k) || marchingGrid->isSprayerCell(i, j, k))
				{
					marchingGrid->phi[i][j][k] = 0;
				}
				else if (marchingGrid->isFluidCell(i, j, k))
				{
					marchingGrid->phi[i][j][k] = -MAX_DOUBLE;
				}
				else if (marchingGrid->isAirCell(i, j, k))
				{
					marchingGrid->phi[i][j][k] = MAX_DOUBLE;
				}
#else
				if (signedAbs(marchingGrid->phi[i][j][k]) <= 2 * marchingGrid->stepSize)
				{
					if (marchingGrid->type[i][j][k] == GT_Air)
					{
						vector3 norm;
						PLS::GetSingleton()->GetNormal(norm, marchingGrid->GetGridCenter(i, j, k));
						vector3 interpolatePos = marchingGrid->GetGridCenter(i, j, k) - signedAbs(marchingGrid->phi[i][j][k]) * norm;
						vector3 velocity = interpolateVelocity(interpolatePos);
						marchingGrid->u[i][j][k] = marchingGrid->u[i + 1][j][k] = velocity.x;
						marchingGrid->v[i][j][k] = marchingGrid->v[i][j + 1][k] = velocity.y;
						marchingGrid->w[i][j][k] = marchingGrid->w[i][j][k + 1] = velocity.z;
					}
				}
#endif
			}
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				if (signedAbs(marchingGrid->phi[i][j][k]) > 2 * marchingGrid->stepSize)
				{
					marchingGrid->phi[i][j][k] = marchingGrid->phi[i][j][k] >= 0 ? MAX_DOUBLE : -MAX_DOUBLE;
				}
			}
		}
	}
}


void NSSolver::fastMarching(GridField* marchingGrid, DOUBLE marchingDist)
{
	vector<Index3> closeContainer;

	INT numX = marchingGrid->numXElement;
	INT numY = marchingGrid->numYElement;
	INT numZ = marchingGrid->numZElement;

	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				if (signedAbs(marchingGrid->phi[i][j][k]) < MAX_DOUBLE)
				{
					if (i - 1 >= 0 && signedAbs(marchingGrid->phi[i - 1][j][k]) >= MAX_DOUBLE)
						closeContainer.push_back(Index3(i - 1, j, k));
					if (i + 1 < numX && signedAbs(marchingGrid->phi[i + 1][j][k]) >= MAX_DOUBLE)
						closeContainer.push_back(Index3(i + 1, j, k));
					if (j - 1 >= 0 && signedAbs(marchingGrid->phi[i][j - 1][k]) >= MAX_DOUBLE)
						closeContainer.push_back(Index3(i, j - 1, k));
					if (j + 1 < numY && signedAbs(marchingGrid->phi[i][j + 1][k]) >= MAX_DOUBLE)
						closeContainer.push_back(Index3(i, j + 1, k));
					if (k - 1 >= 0 && signedAbs(marchingGrid->phi[i][j][k - 1]) >= MAX_DOUBLE)
						closeContainer.push_back(Index3(i, j, k - 1));
					if (k + 1 < numZ && signedAbs(marchingGrid->phi[i][j][k + 1]) >= MAX_DOUBLE)
						closeContainer.push_back(Index3(i, j, k + 1));
				}
			}
		}
	}

	heap.clear();
	for (vector<Index3>::iterator it = closeContainer.begin(); it != closeContainer.end(); ++it)
	{
		INT i = (*it).m_i;
		INT j = (*it).m_j;
		INT k = (*it).m_k;
		assert(i >= 0 && i < numX && j >= 0 && j < numY && k >= 0 && k < numZ);
		LevelSetWithIndex closeElem(marchingGrid->phi[i][j][k], i, j, k);
		if (!heap.exist(Index3(i, j, k)))
		{
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
	}
	while (TRUE)
	{
		if (!heap.size())
			return;

		LevelSetWithIndex heapTop = heap.pop();


		INT i = heapTop.m_i;
		INT j = heapTop.m_j;
		INT k = heapTop.m_k;
		if (signedAbs(heapTop.m_phi) > signedAbs(marchingGrid->phi[i][j][k]))
			continue;
		if (signedAbs(heapTop.m_phi) < MAX_DOUBLE && signedAbs(heapTop.m_phi) > marchingDist)
		{
			break;
		}

		if (i - 1 >= 0 && signedAbs(marchingGrid->phi[i - 1][j][k]) > signedAbs(heapTop.m_phi))
		{
			LevelSetWithIndex closeElem(marchingGrid->phi[i - 1][j][k], i - 1, j, k);
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
		if (i + 1 < numX && signedAbs(marchingGrid->phi[i + 1][j][k]) > signedAbs(heapTop.m_phi))
		{
			LevelSetWithIndex closeElem(marchingGrid->phi[i + 1][j][k], i + 1, j, k);
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
		if (j - 1 >= 0 && signedAbs(marchingGrid->phi[i][j - 1][k]) > signedAbs(heapTop.m_phi))
		{
			LevelSetWithIndex closeElem(marchingGrid->phi[i][j - 1][k], i, j - 1, k);
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
		if (j + 1 < numY && signedAbs(marchingGrid->phi[i][j + 1][k]) > signedAbs(heapTop.m_phi))
		{
			LevelSetWithIndex closeElem(marchingGrid->phi[i][j + 1][k], i, j + 1, k);
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
		if (k - 1 >= 0 && signedAbs(marchingGrid->phi[i][j][k - 1]) > signedAbs(heapTop.m_phi))
		{
			LevelSetWithIndex closeElem(marchingGrid->phi[i][j][k - 1], i, j, k - 1);
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
		if (k + 1 < numZ && signedAbs(marchingGrid->phi[i][j][k + 1]) > signedAbs(heapTop.m_phi))
		{
			LevelSetWithIndex closeElem(marchingGrid->phi[i][j][k + 1], i, j, k + 1);
			calculateCloseElement(marchingGrid, closeElem);
			heap.push(closeElem);
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				if (signedAbs(marchingGrid->phi[i][j][k]) < MAX_DOUBLE && signedAbs(marchingGrid->phi[i][j][k]) > marchingDist)
					marchingGrid->phi[i][j][k] = marchingGrid->phi[i][j][k] >= 0 ? MAX_DOUBLE : -MAX_DOUBLE;
			}
		}
	}
}

void NSSolver::calculateCloseElement(GridField* marchingGrid, LevelSetWithIndex& elem)
{
	elem.m_phi = fastSweepThroughOneElement(marchingGrid, elem.m_i, elem.m_j, elem.m_k);
}

//Edit by YangZhanxin 2007/08/08 Start
static bool UDLesser(pair<DOUBLE, INT> elem1, pair<DOUBLE, INT> elem2)
{
	return elem1.first < elem2.first; //use smaller than to implement greater in order to sort from small to big
}
//Edit by YangZhanxin 2007/08/08 End
DOUBLE NSSolver::fastSweepThroughOneElement(GridField* marchingGrid, INT i, INT j, INT k)
{
	//Edit by YangZhanxin 2007/08/08 Start

	INT numX = marchingGrid->numXElement;
	INT numY = marchingGrid->numYElement;
	INT numZ = marchingGrid->numZElement;

	INT sign = marchingGrid->phi[i][j][k] >= 0 ? 1 : -1;
	DOUBLE result = signedAbs(marchingGrid->phi[i][j][k]);
	DOUBLE*** u = marchingGrid->u;
	DOUBLE*** v = marchingGrid->v;
	DOUBLE*** w = marchingGrid->w;
	DOUBLE*** phi = marchingGrid->phi;
	GridType*** type = marchingGrid->type;
	vector<pair<DOUBLE, INT> > abcs;
//  [4/24/2008 HATEVOL] ANNOTATED start
	//DOUBLE minXdir = min((i - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i - 1][j][k]), (i + 1) >= numX ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i + 1][j][k]));
	//DOUBLE minYdir = min((j - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j - 1][k]), (j + 1) >= numY ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j + 1][k]));
	//DOUBLE minZdir = min((k - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j][k - 1]), (k + 1) >= numZ ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j][k + 1]));

	//INT xDirIndex = ((i - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i - 1][j][k])) > ((i + 1) >= numX ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i + 1][j][k])) ? 1 : -1;
	//INT yDirIndex = ((j - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j - 1][k])) > ((j + 1) >= numY ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j + 1][k])) ? 2 : -2;
	//INT zDirIndex = ((k - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j][k - 1])) > ((k + 1) >= numZ ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j][k + 1])) ? 3 : -3; //1: x, 2: y, 3: z
//  [4/24/2008 HATEVOL] ANNOTATED end
//  [4/24/2008 HATEVOL] ADDED start
	DOUBLE absPhiIminusJK = (i - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i-1][j][k]),
		   absPhiIJminusK = (j - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j-1][k]),
		   absPhiIJKminus = (k - 1) < 0 ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j][k-1]),
		   absPhiIJKplus  = (k + 1) >= numX ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j][k+1]),
		   absPhiIJplusK  = (j + 1) >= numY ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i][j+1][k]),
		   absPhiIplusJK  = (i + 1) >= numZ ? MAX_DOUBLE : signedAbs(marchingGrid->phi[i+1][j][k]);
		   
	DOUBLE minXdir = min(absPhiIminusJK, absPhiIplusJK);
	DOUBLE minYdir = min(absPhiIJminusK, absPhiIJplusK);
	DOUBLE minZdir = min(absPhiIJKminus, absPhiIJKplus);

	INT xDirIndex = (absPhiIminusJK > absPhiIplusJK) ? 1 : -1;
	INT yDirIndex = (absPhiIJminusK > absPhiIJplusK) ? 2 : -2;
	INT zDirIndex = (absPhiIJKminus > absPhiIJKplus) ? 3 : -3; //1: x, 2: y, 3: z

//  [4/24/2008 HATEVOL] ADDED end
	pair<DOUBLE, INT> xDir(minXdir, xDirIndex);
	pair<DOUBLE, INT> yDir(minYdir, yDirIndex);
	pair<DOUBLE, INT> zDir(minZdir, zDirIndex);

	abcs.push_back(xDir);
	abcs.push_back(yDir);
	abcs.push_back(zDir);
	sort(abcs.begin(), abcs.end(), UDLesser);

	DOUBLE a = abcs[0].first;
	DOUBLE b = abcs[1].first;
	DOUBLE c = abcs[2].first;
	DOUBLE h = marchingGrid->stepSize;
	INT axisDir1 = abcs[0].second;
	INT axisDir2 = abcs[1].second;
	INT axisDir3 = abcs[2].second;
	assert(a >= 0 && b >= 0 && c >= 0);
	DOUBLE phiNew = a + static_cast<DOUBLE>(h);
	
	INT xTmpIndex = 0, yTmpIndex = 0, zTmpIndex = 0;
	if (/*marchingGrid->isSolidCell(i, j, k) || */marchingGrid->isAirCell(i, j, k))
	{
		switch(signedAbs(axisDir1)) {
		case 1:
			assert(i + signedAbs(axisDir1) / axisDir1 >= 0 && i + signedAbs(axisDir1) / axisDir1 < static_cast<INT>(numX));
			xTmpIndex = i + ((axisDir1 >= 0) ? 1 : -1);
			yTmpIndex = j;
			zTmpIndex = k;
			break;
		case 2:
			assert(j + signedAbs(axisDir1) / axisDir1 >= 0 && j + signedAbs(axisDir1) / axisDir1 < static_cast<INT>(numY));
			xTmpIndex = i;
			yTmpIndex = j + ((axisDir1 >= 0) ? 1 : -1);
			zTmpIndex = k;
			break;
		case 3:
			assert(k + signedAbs(axisDir1) / axisDir1 >= 0 && k + signedAbs(axisDir1) / axisDir1 < static_cast<INT>(numZ));
			xTmpIndex = i;
			yTmpIndex = j;
			zTmpIndex = k + ((axisDir1 >= 0) ? 1 : -1);
			break;
		default:
			break;
		}
	}
	if (phiNew <= b)
	{
		if (signedAbs(phi[i][j][k]) > phiNew)
		{
			phi[i][j][k] = sign * phiNew;
			result = phiNew;
			if (/*marchingGrid->isSolidCell(i, j, k) || */marchingGrid->isAirCell(i, j, k))
			{
				//if (abs(phi[xTmpIndex][yTmpIndex][zTmpIndex] - MAX_DOUBLE) > EPSILON)
				{	
					if (!marchingGrid->isSolidCell(i - 1, j, k) && !marchingGrid->isVelocityValid('u', i, j, k))
						u[i][j][k] = u[xTmpIndex][yTmpIndex][zTmpIndex];
					if (!marchingGrid->isSolidCell(i, j - 1, k) && !marchingGrid->isVelocityValid('v', i, j , k))
						v[i][j][k] = v[xTmpIndex][yTmpIndex][zTmpIndex];
					if (!marchingGrid->isSolidCell(i, j, k - 1) && !marchingGrid->isVelocityValid('w', i, j, k ))
						w[i][j][k] = w[xTmpIndex][yTmpIndex][zTmpIndex];
					if (!marchingGrid->isSolidCell(i + 1, j, k) && !marchingGrid->isVelocityValid('u', i + 1, j, k))
						u[i + 1][j][k] = u[xTmpIndex + 1][yTmpIndex][zTmpIndex];
					if (!marchingGrid->isSolidCell(i, j + 1, k) && !marchingGrid->isVelocityValid('v', i, j + 1, k))
						v[i][j + 1][k] = v[xTmpIndex][yTmpIndex + 1][zTmpIndex];
					if (!marchingGrid->isSolidCell(i, j, k + 1) && !marchingGrid->isVelocityValid('w', i, j, k + 1))
						w[i][j][k + 1] = w[xTmpIndex][yTmpIndex][zTmpIndex + 1];
				}
			}
		}
	}
	else
	{
		DOUBLE sqrtPart = sqrt((a + b) * (a + b) - 2 * (a * a + b * b - h * h));	
		DOUBLE phiNewLarge = (a + b + sqrtPart) / 2;
		phiNew = phiNewLarge;
		INT xTmpIndex2 = 0, yTmpIndex2 = 0, zTmpIndex2 = 0;
		if (/*marchingGrid->isSolidCell(i, j, k) || */marchingGrid->isAirCell(i, j, k))
		{
			switch(signedAbs(axisDir2)) {
			case 1:
				assert(i + signedAbs(axisDir2) / axisDir2 >= 0 && i + signedAbs(axisDir2) / axisDir2 < static_cast<INT>(numX));
				xTmpIndex2 = i + ((axisDir2 >= 0) ? 1 : -1);
				yTmpIndex2 = j;
				zTmpIndex2 = k;				
				break;
			case 2:
				assert(j + signedAbs(axisDir2) / axisDir2 >= 0 && j + signedAbs(axisDir2) / axisDir2 < static_cast<INT>(numY));
				xTmpIndex2 = i;
				yTmpIndex2 = j + ((axisDir2 >= 0) ? 1 : -1);
				zTmpIndex2 = k;
				break;
			case 3:
				assert(k + signedAbs(axisDir2) / axisDir2 >= 0 && k + signedAbs(axisDir2) / axisDir2 < static_cast<INT>(numZ));
				xTmpIndex2 = i;
				yTmpIndex2 = j;
				zTmpIndex2 = k + ((axisDir2 >= 0) ? 1 : -1);
				break;
			default:
				assert(0);
				break;
			}
		}
		if (phiNew <= c)
		{
			if (signedAbs(phi[i][j][k]) > phiNew)
			{
				phi[i][j][k] = sign * phiNew;
				result = phiNew;
				if (/*marchingGrid->isSolidCell(i, j, k) || */marchingGrid->isAirCell(i, j, k))
				{
					//if (abs(phi[xTmpIndex][yTmpIndex][zTmpIndex] - MAX_DOUBLE) > EPSILON && abs(phi[xTmpIndex2][yTmpIndex2][zTmpIndex2] - MAX_DOUBLE) > EPSILON)
					{
						DOUBLE uTmpIndex = (u[xTmpIndex][yTmpIndex][zTmpIndex] + u[xTmpIndex + 1][yTmpIndex][zTmpIndex]) / 2;
						DOUBLE vTmpIndex = (v[xTmpIndex][yTmpIndex][zTmpIndex] + v[xTmpIndex][yTmpIndex + 1][zTmpIndex]) / 2;
						DOUBLE wTmpIndex = (w[xTmpIndex][yTmpIndex][zTmpIndex] + w[xTmpIndex][yTmpIndex][zTmpIndex + 1]) / 2;
						DOUBLE uTmpIndex2 = (u[xTmpIndex2][yTmpIndex2][zTmpIndex2] + u[xTmpIndex2 + 1][yTmpIndex2][zTmpIndex2]) / 2;
						DOUBLE vTmpIndex2 = (v[xTmpIndex2][yTmpIndex2][zTmpIndex2] + v[xTmpIndex2][yTmpIndex2 + 1][zTmpIndex2]) / 2;
						DOUBLE wTmpIndex2 = (w[xTmpIndex2][yTmpIndex2][zTmpIndex2] + w[xTmpIndex2][yTmpIndex2][zTmpIndex2 + 1]) / 2;

						DOUBLE tmpU = (uTmpIndex * (phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + uTmpIndex2 * (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]))
							/ ((phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]));
						DOUBLE tmpV = (vTmpIndex * (phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + vTmpIndex2 * (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]))
							/ ((phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]));
						DOUBLE tmpW = (wTmpIndex * (phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + wTmpIndex2 * (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]))
							/ ((phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]));
						
						
						if (!marchingGrid->isSolidCell(i - 1, j, k) && !marchingGrid->isVelocityValid('u', i, j, k))
							u[i][j][k] = tmpU;
						if (!marchingGrid->isSolidCell(i, j - 1, k) && !marchingGrid->isVelocityValid('v', i, j , k))
							v[i][j][k] = tmpV;
						if (!marchingGrid->isSolidCell(i, j, k - 1) && !marchingGrid->isVelocityValid('w', i, j, k ))
							w[i][j][k] = tmpW;
						if (!marchingGrid->isSolidCell(i + 1, j, k) && !marchingGrid->isVelocityValid('u', i + 1, j, k))
							u[i + 1][j][k] = tmpU;
						if (!marchingGrid->isSolidCell(i, j + 1, k) && !marchingGrid->isVelocityValid('v', i, j + 1, k))
							v[i][j + 1][k] = tmpV;
						if (!marchingGrid->isSolidCell(i, j, k + 1) && !marchingGrid->isVelocityValid('w', i, j, k + 1))
							w[i][j][k + 1] = tmpW;
					
					}
				}
			}
		}
		else
		{
			sqrtPart = sqrt((a + b + c) * (a + b + c) - 3 * (a * a + b * b + c * c - h * h));		
			phiNewLarge = (a + b + c + sqrtPart) / 3;
			phiNew = sign * phiNewLarge;
			if (signedAbs(phi[i][j][k]) > phiNew)
			{
				phi[i][j][k] = phiNew;
				result = phiNew;
				if (/*marchingGrid->isSolidCell(i, j, k) || */marchingGrid->isAirCell(i, j, k))
				{
					INT xTmpIndex3 = 0, yTmpIndex3 = 0, zTmpIndex3 = 0;
					switch(signedAbs(axisDir3))
					{
					case 1:
						assert(i + signedAbs(axisDir3) / axisDir3 >= 0 && i + signedAbs(axisDir3) / axisDir3 < static_cast<INT>(numX));
						xTmpIndex3 = i + ((axisDir3 >= 0) ? 1 : -1);
						yTmpIndex3 = j;
						zTmpIndex3 = k;	
						break;
					case 2:
						assert(j + signedAbs(axisDir3) / axisDir3 >= 0 && j + signedAbs(axisDir3) / axisDir3 < static_cast<INT>(numY));
						xTmpIndex3 = i;
						yTmpIndex3 = j + ((axisDir3 >= 0) ? 1 : -1);
						zTmpIndex3 = k;
						break;
					case 3:
						assert(k + signedAbs(axisDir3) / axisDir3 >= 0 && k + signedAbs(axisDir3) / axisDir3 < static_cast<INT>(numZ));
						xTmpIndex3 = i;
						yTmpIndex3 = j;
						zTmpIndex3 = k + ((axisDir3 >= 0) ? 1 : -1);
						break;
					default:
						assert(0);
						break;
					}
					//if (abs(phi[xTmpIndex][yTmpIndex][zTmpIndex] - MAX_DOUBLE) > EPSILON && abs(phi[xTmpIndex2][yTmpIndex2][zTmpIndex2] - MAX_DOUBLE) > EPSILON && abs(phi[xTmpIndex3][yTmpIndex3][zTmpIndex3] - MAX_DOUBLE) > EPSILON)
					{
						DOUBLE uTmpIndex = (u[xTmpIndex][yTmpIndex][zTmpIndex] + u[xTmpIndex + 1][yTmpIndex][zTmpIndex]) / 2;
						DOUBLE vTmpIndex = (v[xTmpIndex][yTmpIndex][zTmpIndex] + v[xTmpIndex][yTmpIndex + 1][zTmpIndex]) / 2;
						DOUBLE wTmpIndex = (w[xTmpIndex][yTmpIndex][zTmpIndex] + w[xTmpIndex][yTmpIndex][zTmpIndex + 1]) / 2;
						DOUBLE uTmpIndex2 = (u[xTmpIndex2][yTmpIndex2][zTmpIndex2] + u[xTmpIndex2 + 1][yTmpIndex2][zTmpIndex2]) / 2;
						DOUBLE vTmpIndex2 = (v[xTmpIndex2][yTmpIndex2][zTmpIndex2] + v[xTmpIndex2][yTmpIndex2 + 1][zTmpIndex2]) / 2;
						DOUBLE wTmpIndex2 = (w[xTmpIndex2][yTmpIndex2][zTmpIndex2] + w[xTmpIndex2][yTmpIndex2][zTmpIndex2 + 1]) / 2;
						DOUBLE uTmpIndex3 = (u[xTmpIndex3][yTmpIndex3][zTmpIndex3] + u[xTmpIndex3 + 1][yTmpIndex3][zTmpIndex3]) / 2;
						DOUBLE vTmpIndex3 = (v[xTmpIndex3][yTmpIndex3][zTmpIndex3] + v[xTmpIndex3][yTmpIndex3 + 1][zTmpIndex3]) / 2;
						DOUBLE wTmpIndex3 = (w[xTmpIndex3][yTmpIndex3][zTmpIndex3] + w[xTmpIndex3][yTmpIndex3][zTmpIndex3 + 1]) / 2;

						DOUBLE tmpU = (uTmpIndex * (phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + uTmpIndex2 * (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]) + uTmpIndex3 * (phi[i][j][k] - phi[xTmpIndex3][yTmpIndex3][zTmpIndex3]))
									/ ((phi[i][j][k] * 3 - phi[xTmpIndex][yTmpIndex][zTmpIndex] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2] - phi[xTmpIndex3][yTmpIndex3][zTmpIndex3]));
						DOUBLE tmpV = (vTmpIndex * (phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + vTmpIndex2 * (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]) + vTmpIndex3 * (phi[i][j][k] - phi[xTmpIndex3][yTmpIndex3][zTmpIndex3]))
									/ ((phi[i][j][k] * 3 - phi[xTmpIndex][yTmpIndex][zTmpIndex] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2] - phi[xTmpIndex3][yTmpIndex3][zTmpIndex3]));
						DOUBLE tmpW = (wTmpIndex * (phi[i][j][k] - phi[xTmpIndex][yTmpIndex][zTmpIndex]) + wTmpIndex2 * (phi[i][j][k] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2]) + wTmpIndex3 * (phi[i][j][k] - phi[xTmpIndex3][yTmpIndex3][zTmpIndex3]))
									/ ((phi[i][j][k] * 3 - phi[xTmpIndex][yTmpIndex][zTmpIndex] - phi[xTmpIndex2][yTmpIndex2][zTmpIndex2] - phi[xTmpIndex3][yTmpIndex3][zTmpIndex3]));

						if (!marchingGrid->isSolidCell(i - 1, j, k) && !marchingGrid->isVelocityValid('u', i, j, k))
							u[i][j][k] = tmpU;
						if (!marchingGrid->isSolidCell(i, j - 1, k) && !marchingGrid->isVelocityValid('v', i, j , k))
							v[i][j][k] = tmpV;
						if (!marchingGrid->isSolidCell(i, j, k - 1) && !marchingGrid->isVelocityValid('w', i, j, k ))
							w[i][j][k] = tmpW;
						if (!marchingGrid->isSolidCell(i + 1, j, k) && !marchingGrid->isVelocityValid('u', i + 1, j, k))
							u[i + 1][j][k] = tmpU;
						if (!marchingGrid->isSolidCell(i, j + 1, k) && !marchingGrid->isVelocityValid('v', i, j + 1, k))
							v[i][j + 1][k] = tmpV;
						if (!marchingGrid->isSolidCell(i, j, k + 1) && !marchingGrid->isVelocityValid('w', i, j, k + 1))
							w[i][j][k + 1] = tmpW;
					}
				}
			}
		}
	}

	return sign * result;


	//Edit by YangZhanxin 2007/08/08 End
}
void NSSolver::fastSweepAllDirs()
{
	//Edit by YangZhanxin 2007/08/08 Start
	INT numX = static_cast<INT>(m_grid->numXElement);
	INT numY = static_cast<INT>(m_grid->numYElement);
	INT numZ = static_cast<INT>(m_grid->numZElement);
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = numZ - 1; k >= 0; k--)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = numY - 1; j >= 0 ; j--)
		{
			for (INT k = 0; k < numZ; k++)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = 0; i < numX; i++)
	{
		for (INT j = numY - 1; j >= 0 ; j--)
		{
			for (INT k = numZ - 1; k >= 0; k--)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = numX - 1; i >= 0 ; i--)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = numX - 1; i >= 0 ; i--)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = numZ - 1; k >= 0; k--)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = numX - 1; i >= 0 ; i--)
	{
		for (INT j = numY - 1; j >= 0 ; j--)
		{
			for (INT k = 0; k < numZ; k++)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	for (INT i = numX - 1; i >= 0 ; i--)
	{
		for (INT j = numY - 1; j >= 0 ; j--)
		{
			for (INT k = numZ - 1; k >= 0; k--)
			{
				fastSweepThroughOneElement(m_grid, i, j, k);
			}
		}
	}
	//Edit by YangZhanxin 2007/08/08 End
}

#ifdef _PARTICLES
void NSSolver::updateGridType(GridField* grid)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;
	DOUBLE stepSize = grid->stepSize;
	CopyTo3DArray(grid->prevInDomain, grid->inDomain, numX, numY, numZ);
	//grid->ClearTypeFlagTo(GT_Air);
	grid->m_compDomain.clear();
	grid->m_validU.clear();
	grid->m_validV.clear();
	grid->m_validW.clear();

	const list<Particle>& markerParticles = m_scene->particles.GetAllParticles();
	for (list<Particle>::const_iterator it = markerParticles.begin(); it != markerParticles.end(); ++it)
	{
		vector3 pos = (*it).m_pos;
		UINT xIndex, yIndex, zIndex;
		grid->fromWorldToLocalGrid(pos, xIndex, yIndex, zIndex);
		if (grid->type[xIndex][yIndex][zIndex] != GT_Solid && grid->type[xIndex][yIndex][zIndex] != GT_Sprayer)
		{
			grid->type[xIndex][yIndex][zIndex] = GT_FLuid;
//  [4/16/2008 HATEVOL] ADDED start
			grid->fastType[xIndex+1][yIndex+1][zIndex+1] = GT_FLuid;
//  [4/16/2008 HATEVOL] ADDED end
			if (!grid->inDomain[xIndex][yIndex][zIndex])
			{
				grid->inDomain[xIndex][yIndex][zIndex] = TRUE;
				grid->m_compDomain.push_back(Index3(xIndex, yIndex, zIndex));
			}
		}
	}
	UINT numIndices = static_cast<INT>(grid->m_compDomain.size());

	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;

		if (grid->isFluidCell(i, j, k))
		{
			if (grid->isAirCell(i - 1, j, k) || grid->isAirCell(i + 1, j, k) ||
				grid->isAirCell(i, j - 1, k) || grid->isAirCell(i, j + 1, k) ||
				grid->isAirCell(i, j, k - 1) || grid->isAirCell(i, j, k + 1))
			{

				grid->type[i][j][k] = GT_Surface;
//  [4/16/2008 HATEVOL] ADDED start
				grid->fastType[i+1][j+1][k+1] = GT_Surface;
//  [4/16/2008 HATEVOL] ADDED end
				if (!grid->inDomain[i][j][k])
				{
					grid->inDomain[i][j][k] = TRUE;
					grid->m_compDomain.push_back(Index3(i, j, k));
				}
			}
		}
	}

	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;


		if ( !tmpDomain[i][j][k] && !grid->isSolidCell(i - 1, j, k) && !grid->isSprayerCell(i - 1, j, k))
		{
			grid->m_validU.push_back(Index3(i, j, k));
			tmpDomain[i][j][k] = TRUE;
		}

		if ( !tmpDomain[i + 1][j][k] && !grid->isSolidCell(i + 1, j, k) && !grid->isSprayerCell(i + 1, j, k))
		{
			grid->m_validU.push_back(Index3(i + 1, j, k));
			tmpDomain[i + 1][j][k] = TRUE;
		}
	}
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;

		if ( !tmpDomain[i][j][k] && !grid->isSolidCell(i, j - 1, k) && !grid->isSprayerCell(i, j - 1, k))
		{
			grid->m_validV.push_back(Index3(i, j, k));
			tmpDomain[i][j][k] = TRUE;
		}

		if ( !tmpDomain[i][j + 1][k] && !grid->isSolidCell(i, j + 1, k) && !grid->isSprayerCell(i, j + 1, k))
		{
			grid->m_validV.push_back(Index3(i, j + 1, k));
			tmpDomain[i][j + 1][k] = TRUE;
		}
	}
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;

		if ( !tmpDomain[i][j][k] && !grid->isSolidCell(i, j, k - 1) && !grid->isSprayerCell(i, j, k - 1))
		{
			grid->m_validW.push_back(Index3(i, j, k));
			tmpDomain[i][j][k] = TRUE;
		}
		if ( !tmpDomain[i][j][k + 1] && !grid->isSolidCell(i, j, k + 1) && !grid->isSprayerCell(i, j, k + 1))
		{
			grid->m_validW.push_back(Index3(i, j, k + 1));
			tmpDomain[i][j][k + 1] = TRUE;
		}
	}
}
#else
void NSSolver::updateGridType(GridField* grid)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;
	DOUBLE stepSize = grid->stepSize;
	//grid->ClearTypeFlagTo(GT_Air);

	grid->m_compDomain.clear();
	grid->m_validU.clear();
	grid->m_validV.clear();
	grid->m_validW.clear();
	INT xElemPerBlock = numX / m_blockSizeX;
	INT yElemPerBlock = numY / m_blockSizeY;
	INT zElemPerBlock = numZ / m_blockSizeZ;
	INT listSize = static_cast<INT>(m_blockToUpdate.size());
	if (!listSize)
	{
		for (INT ithBlock = 0; ithBlock < m_blockSizeX; ithBlock++)
		{
			for (INT jthBlock = 0; jthBlock < m_blockSizeY; jthBlock++)
			{
				for (INT kthBlock = 0; kthBlock < m_blockSizeZ; kthBlock++)
				{
					m_blockToUpdate.push_back(Index3(ithBlock, jthBlock, kthBlock));
				}
			}
		}
	}

	listSize = static_cast<INT>(m_blockToUpdate.size());
	//if (grid == m_grid)
		Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);

	(*DecoLogger::GetSingleton()) << listSize << " blocks are updated!" << "\n";
	DOUBLE aimDist = detailDepth * stepSize;
	for (list<Index3>::const_iterator it = m_blockToUpdate.begin(); it != m_blockToUpdate.end(); ++it)
	{
		//Index3 indices = m_blockToUpdate.front();
		//m_blockToUpdate.pop_front();

		INT ithBlock = (*it).m_i;
		INT jthBlock = (*it).m_j;
		INT kthBlock = (*it).m_k;
		INT startI = ithBlock * xElemPerBlock;
		INT endI = (ithBlock + 1) * xElemPerBlock;
		INT startJ = jthBlock * yElemPerBlock;
		INT endJ = (jthBlock + 1) * yElemPerBlock;
		INT startK = kthBlock * zElemPerBlock;
		INT endK = (kthBlock + 1) * zElemPerBlock;
		for (INT i = startI; i < endI; i++)
		{
			for (INT j = startJ; j < endJ; j++)
			{
				for (INT k = startK; k < endK; k++)
				{
#ifdef _SLC
					grid->phi[i][j][k] = MAX_DOUBLE;
#elif _DENSITY
					grid->phi[i][j][k] = 0;
#endif
					if (!grid->isSolidCell(i, j, k) && !grid->isSprayerCell(i, j, k))
					{
#ifdef _SLC
						DOUBLE signedDist = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i, j, k));
						grid->phi[i][j][k] = signedDist;
						if (signedDist <= 0)
#elif _DENSITY
						DOUBLE rho = DensityField::GetSingleton()->SampleRho(grid->GetGridCenter(i, j, k));
						grid->phi[i][j][k] = rho;
						if (rho > EPSILON_FLOAT)
#endif
						{
							grid->type[i][j][k] = GT_FLuid;
							//  [4/16/2008 HATEVOL] ADDED start
							grid->fastType[i+1][j+1][k+1] = GT_FLuid;
							//  [4/16/2008 HATEVOL] ADDED end
							for (INT ithDir = -1; ithDir <= 1; ithDir++)
							{
								for (INT jthDir = -1; jthDir <= 1; jthDir++)
								{
									for (INT kthDir = -1; kthDir <= 1; kthDir++)
									{
										INT ithIndex = ithBlock + ithDir;
										INT jthIndex = jthBlock + jthDir;
										INT kthIndex = kthBlock + kthDir;
										if (ithIndex >= 0 && ithIndex < m_blockSizeX && jthIndex >= 0 && jthIndex < m_blockSizeY && kthIndex >= 0 && kthIndex < m_blockSizeZ)
											tmpDomain[ithIndex][jthIndex][kthIndex] = TRUE;
									}
								}
							}

							if (grid == m_grid)
							{
								if (!grid->inDomain[i][j][k])
								{
									grid->inDomain[i][j][k] = TRUE;
									grid->m_compDomain.push_back(Index3(i, j, k));
								}
							}
						}
					}
				}
			}
		}
	}


	{
		m_blockToUpdate.clear();
		for (INT ithBlock = 0; ithBlock < m_blockSizeX; ithBlock++)
		{
			for (INT jthBlock = 0; jthBlock < m_blockSizeY; jthBlock++)
			{
				for (INT kthBlock = 0; kthBlock < m_blockSizeZ; kthBlock++)
				{
					if (tmpDomain[ithBlock][jthBlock][kthBlock])
						m_blockToUpdate.push_back(Index3(ithBlock, jthBlock, kthBlock));
				}
			}
		}
	}
	/////////////temp hack for thin water sheet and wrong air in water
	vector<vector3> surfaceMarker;
	SLC::GetSingleton()->GetSurfaceMarker(surfaceMarker);
	INT numMarkers = static_cast<INT>(surfaceMarker.size());
	vector<pair<Index3, GridType> > hackContainer;
	hackContainer.clear();
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);

	for (INT i = 0; i < numMarkers; i++)
	{
		UINT xIndex, yIndex, zIndex;
		vector3 pos = surfaceMarker[i];
		grid->fromWorldToLocalGrid(pos, xIndex, yIndex, zIndex);
		if (tmpDomain[xIndex][yIndex][zIndex])
			continue;
		tmpDomain[xIndex][yIndex][zIndex] = TRUE;

		if (!grid->isSolidCell(xIndex, yIndex, zIndex) && !grid->isSprayerCell(xIndex, yIndex, zIndex))
		{
			if (grid->IsThinSheetCell(xIndex, yIndex, zIndex))
			{
				hackContainer.push_back(pair<Index3, GridType>(Index3(xIndex, yIndex, zIndex), GT_Surface));
			}

		}
	}
	INT numHack = static_cast<INT>(hackContainer.size());
	for (INT it = 0; it < numHack; it++)
	{
		INT xIndex = hackContainer[it].first.m_i;
		INT yIndex = hackContainer[it].first.m_j;
		INT zIndex = hackContainer[it].first.m_k;
		if (!grid->inDomain[xIndex][yIndex][zIndex])
		{
			grid->inDomain[xIndex][yIndex][zIndex] = TRUE;
			grid->phi[xIndex][yIndex][zIndex] = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(xIndex, yIndex, zIndex));
			grid->m_compDomain.push_back(Index3(xIndex, yIndex, zIndex));
			grid->type[xIndex][yIndex][zIndex] = hackContainer[it].second;
			grid->fastType[xIndex + 1][yIndex + 1][zIndex + 1] = hackContainer[it].second;
		}

	}

	UINT numIndices = static_cast<INT>(grid->m_compDomain.size());

	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;
		if (grid->isFluidCell(i, j, k))
		{
			//if (grid->isAirCell(i - 1, j, k) || grid->isAirCell(i + 1, j, k) ||
			//	grid->isAirCell(i, j - 1, k) || grid->isAirCell(i, j + 1, k) ||
			//	grid->isAirCell(i, j, k - 1) || grid->isAirCell(i, j, k + 1))
			if (grid->phi[i][j][k] <= 0 && 
				(!grid->isSolidCell(i - 1, j, k) && !grid->isSprayerCell(i - 1, j, k) && grid->type[i - 1][j][k] != GT_FLuid && grid->phi[i - 1][j][k] > 0 || 
				 !grid->isSolidCell(i, j - 1, k) && !grid->isSprayerCell(i, j - 1, k) && grid->type[i][j - 1][k] != GT_FLuid && grid->phi[i][j - 1][k] > 0 || 
				 !grid->isSolidCell(i, j, k - 1) && !grid->isSprayerCell(i, j, k - 1) && grid->type[i][j][k - 1] != GT_FLuid && grid->phi[i][j][k - 1] > 0 ||
				 !grid->isSolidCell(i + 1, j, k) && !grid->isSprayerCell(i + 1, j, k) && grid->type[i + 1][j][k] != GT_FLuid && grid->phi[i + 1][j][k] > 0 || 
				 !grid->isSolidCell(i, j + 1, k) && !grid->isSprayerCell(i, j + 1, k) && grid->type[i][j + 1][k] != GT_FLuid && grid->phi[i][j + 1][k] > 0 || 
				 !grid->isSolidCell(i, j, k + 1) && !grid->isSprayerCell(i, j, k + 1) && grid->type[i][j][k + 1] != GT_FLuid && grid->phi[i][j][k + 1] > 0))
			{

				grid->type[i][j][k] = GT_Surface;
				//  [4/16/2008 HATEVOL] ADDED start
				grid->fastType[i+1][j+1][k+1] = GT_Surface;
				//  [4/16/2008 HATEVOL] ADDED end
				if (!grid->inDomain[i][j][k])
				{
					grid->inDomain[i][j][k] = TRUE;
					grid->m_compDomain.push_back(Index3(i, j, k));
				}
			}
		}
	}
	numIndices = static_cast<INT>(grid->m_compDomain.size());
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;


		if ( !tmpDomain[i][j][k] && !grid->isSolidCell(i - 1, j, k) && !grid->isSprayerCell(i - 1, j, k))
		{
			grid->m_validU.push_back(Index3(i, j, k));
			tmpDomain[i][j][k] = TRUE;
		}

		if ( !tmpDomain[i + 1][j][k] && !grid->isSolidCell(i + 1, j, k) && !grid->isSprayerCell(i + 1, j, k))
		{
			grid->m_validU.push_back(Index3(i + 1, j, k));
			tmpDomain[i + 1][j][k] = TRUE;
		}
	}
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;
		if ( !tmpDomain[i][j][k] && !grid->isSolidCell(i, j - 1, k) && !grid->isSprayerCell(i, j - 1, k))
		{
			grid->m_validV.push_back(Index3(i, j, k));
			tmpDomain[i][j][k] = TRUE;
		}
		if ( !tmpDomain[i][j + 1][k] && !grid->isSolidCell(i, j + 1, k) && !grid->isSprayerCell(i, j + 1, k))
		{
			grid->m_validV.push_back(Index3(i, j + 1, k));
			tmpDomain[i][j + 1][k] = TRUE;
		}
	}
	Zero3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
	for (UINT it = 0; it < numIndices; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;
		if ( !tmpDomain[i][j][k] && !grid->isSolidCell(i, j, k - 1) && !grid->isSprayerCell(i, j, k - 1))
		{
			grid->m_validW.push_back(Index3(i, j, k));
			tmpDomain[i][j][k] = TRUE;
		}

		if ( !tmpDomain[i][j][k + 1] && !grid->isSolidCell(i, j, k + 1) && !grid->isSprayerCell(i, j, k + 1))
		{
			grid->m_validW.push_back(Index3(i, j, k + 1));
			tmpDomain[i][j][k + 1] = TRUE;
		}
	}
}



#endif

void NSSolver::updateGridType()
{
	updateGridType(m_grid);
}

void NSSolver::formMatrixAndDivergence(GridField* grid, DOUBLE ***ADiag, DOUBLE ***APlusi, DOUBLE ***APlusj, DOUBLE ***APlusk, DOUBLE ***divergenceVelocity, vector<Index3>& indices, BOOL*** domain)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;
	INT numElement = (INT)numX * (INT)numY * (INT)numZ;
	DOUBLE TIME_STEP = m_scene->GetTimeStep();
#ifdef _DEBUG_TIME
	DecoTimer t;
	t.startTimer();
#endif 
	m_tension = 0;

	for (INT i = 0; i < numX; i++)
	{
		for (INT j = 0; j < numY; j++)
		{
			for (INT k = 0; k < numZ; k++)
			{
				if (!domain[i][j][k])
					continue;


				ADiag[i][j][k] = 6;
				APlusi[i][j][k] = APlusj[i][j][k] = APlusk[i][j][k] = -1;
				if (!grid->isBoundaryCell(i, j, k))
				{
					divergenceVelocity[i][j][k] = -((getU(grid, i + 1, j, k) - getU(grid, i, j, k))
						+ (getV(grid, i, j + 1, k) - getV(grid, i, j, k))
						+ (getW(grid, i, j, k + 1) - getW(grid, i, j, k))) * grid->stepSize
						/ (TIME_STEP / RHO_WATER);

					indices.push_back(Index3(i, j, k));
				}
				else
				{
					indices.push_back(Index3(i, j, k));

					vector3 gridCenter = grid->GetGridCenter(i, j, k);

					if (grid->isSolidCell(i - 1, j, k) || grid->isSprayerCell(i - 1, j, k))
					{
						ADiag[i][j][k]--;
						if (i - 1 >= 0 && !domain[i - 1][j][k])
						{
							APlusi[i - 1][j][k] = 0;
						}
					}
					if (grid->isSolidCell(i + 1, j, k) || grid->isSprayerCell(i + 1, j, k))
					{
						ADiag[i][j][k]--;
						APlusi[i][j][k] = 0;
					}
					if (grid->isSolidCell(i, j - 1, k) || grid->isSprayerCell(i, j - 1,k ))
					{
						ADiag[i][j][k]--;
						if (j - 1 >= 0 && !domain[i][j - 1][k])
						{
							APlusj[i][j - 1][k] = 0;
						}
					}
					if (grid->isSolidCell(i, j + 1, k) || grid->isSprayerCell(i, j + 1, k))
					{
						ADiag[i][j][k]--;
						APlusj[i][j][k] = 0;
					}

					if (grid->isSolidCell(i, j, k - 1) || grid->isSprayerCell(i, j, k - 1))
					{
						ADiag[i][j][k]--;
						if (k - 1 >= 0 && !domain[i][j][k - 1])
						{
							APlusk[i][j][k - 1] = 0;
						}
					}
					if (grid->isSolidCell(i, j, k + 1) || grid->isSprayerCell(i, j, k + 1))
					{
						ADiag[i][j][k]--;
						APlusk[i][j][k] = 0;
					}

					if (grid->isAirCell(i - 1, j, k))
					{
						if (i - 1 >= 0 && !domain[i - 1][j][k])
						{
							APlusi[i - 1][j][k] = 0;
						}
					}
					if (grid->isAirCell(i + 1, j, k))
					{
						APlusi[i][j][k] = 0;
					}
					if (grid->isAirCell(i, j - 1, k))
					{
						if (j - 1 >= 0 && !domain[i][j - 1][k])
						{
							APlusj[i][j - 1][k] = 0;
						}
					}
					if (grid->isAirCell(i, j + 1, k))
					{
						APlusj[i][j][k] = 0;
					}

					if (grid->isAirCell(i, j, k - 1))
					{
						if (k - 1 >= 0 && !domain[i][j][k - 1])
						{
							APlusk[i][j][k - 1] = 0;
						}
					}
					if (grid->isAirCell(i, j, k + 1))
					{
						APlusk[i][j][k] = 0;
					}
					divergenceVelocity[i][j][k] = -(getU(grid, i + 1, j, k) - getU(grid, i, j, k) + getV(grid, i, j + 1, k) - getV(grid, i, j, k) + getW(grid, i, j, k + 1) - getW(grid, i, j, k));

					divergenceVelocity[i][j][k] *= grid->stepSize / (TIME_STEP / RHO_WATER);

#ifdef _SLC
					if (grid->isSurfaceCell(i, j, k))
					{
						if (bUseSurfacePressure)
						{
							DOUBLE signedDist = grid->phi[i][j][k];//SLC::GetSingleton()->SamplePhi(gridCenter);
							BOOL bInside = signedDist <= 0;
							if (!bInside)
							{
								domain[i][j][k] = FALSE;
								indices.pop_back();
								continue;
							}
							else
							{
								DOUBLE neibourDist = 0;
								//neibourDist = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i + 1, j, k));
								if (!grid->isSolidCell(i + 1, j, k) && !grid->isSprayerCell(i + 1, j, k) && grid->type[i + 1][j][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i + 1][j][k]) > 0)
								{
									DOUBLE theta = max(signedAbs(signedDist) / (signedAbs(signedDist) + signedAbs(neibourDist)), 1e-6); 
									grid->pTheta[i][j][k].x = theta;
									ADiag[i][j][k] += (1 - theta) / theta;
									APlusi[i][j][k] = 0;
								}
								//neibourDist = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i, j + 1, k));
								if (!grid->isSolidCell(i, j + 1, k) && !grid->isSprayerCell(i, j + 1, k) && grid->type[i][j + 1][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j + 1][k]) > 0)
								{
									DOUBLE theta = max(signedAbs(signedDist) / (signedAbs(signedDist) + signedAbs(neibourDist)), 1e-6); 
									grid->pTheta[i][j][k].y = theta;
									ADiag[i][j][k] += (1 - theta) / theta;
									APlusj[i][j][k] = 0;
								}
								//neibourDist = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i, j, k + 1));
								if (!grid->isSolidCell(i, j, k + 1) && !grid->isSprayerCell(i, j, k + 1) && grid->type[i][j][k + 1] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j][k + 1]) > 0)
								{
									DOUBLE theta = max(signedAbs(signedDist) / (signedAbs(signedDist) + signedAbs(neibourDist)), 1e-6); 
									grid->pTheta[i][j][k].z = theta;
									ADiag[i][j][k] += (1 - theta) / theta;
									APlusk[i][j][k] = 0;
								}
								//neibourDist = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i - 1, j, k));
								if (!grid->isSolidCell(i - 1, j, k) && !grid->isSprayerCell(i - 1, j, k) && grid->type[i - 1][j][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i - 1][j][k]) > 0)
								{
									DOUBLE theta = max(signedAbs(signedDist) / (signedAbs(signedDist) + signedAbs(neibourDist)), 1e-6); 
									grid->mTheta[i][j][k].x = theta;
									ADiag[i][j][k] += (1 - theta) / theta;
									APlusi[i - 1][j][k] = 0;
								}
								//neibourDist = SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i, j - 1, k));
								if (!grid->isSolidCell(i, j - 1, k) && !grid->isSprayerCell(i, j - 1, k) && grid->type[i][j - 1][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j - 1][k]) > 0)
								{
									DOUBLE theta = max(signedAbs(signedDist) / (signedAbs(signedDist) + signedAbs(neibourDist)), 1e-6); 
									grid->mTheta[i][j][k].y = theta;
									ADiag[i][j][k] += (1 - theta) / theta;
									APlusj[i][j - 1][k] = 0;
								}
								//SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i, j, k - 1));
								if (!grid->isSolidCell(i, j, k - 1) && !grid->isSprayerCell(i, j, k - 1) && grid->type[i][j][k - 1] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j][k - 1]) > 0)
								{
									DOUBLE theta = max(signedAbs(signedDist) / (signedAbs(signedDist) + signedAbs(neibourDist)), 1e-6); 
									grid->mTheta[i][j][k].z = theta;
									ADiag[i][j][k] += (1 - theta) / theta;
									APlusk[i][j][k - 1] = 0;
								}
							}
						}
						else
						{
							if (grid->isAirCell(i + 1, j, k))
								APlusi[i][j][k] = 0;
							if (grid->isAirCell(i - 1, j, k))
								APlusi[i - 1][j][k] = 0;
							if (grid->isAirCell(i, j + 1, k))
								APlusj[i][j][k] = 0;
							if (grid->isAirCell(i, j - 1, k))
								APlusj[i][j - 1][k] = 0;
							if (grid->isAirCell(i, j, k + 1))
								APlusk[i][j][k] = 0;
							if (grid->isAirCell(i, j, k - 1))
								APlusk[i][j][k - 1] = 0;
						}
					}
#else
					if (grid->isAirCell(i + 1, j, k))
						APlusi[i][j][k] = 0;
					if (grid->isAirCell(i - 1, j, k))
						APlusi[i - 1][j][k] = 0;
					if (grid->isAirCell(i, j + 1, k))
						APlusj[i][j][k] = 0;
					if (grid->isAirCell(i, j - 1, k))
						APlusj[i][j - 1][k] = 0;
					if (grid->isAirCell(i, j, k + 1))
						APlusk[i][j][k] = 0;
					if (grid->isAirCell(i, j, k - 1))
						APlusk[i][j][k - 1] = 0;
#endif
				}

			}
		}
	}
	DOUBLE norm = 0;

	INT indiceSize = static_cast<INT>(indices.size());
	for (INT index = 0; index < indiceSize; index++)
	{
		INT i = indices[index].m_i;
		INT j = indices[index].m_j;
		INT k = indices[index].m_k;

		norm = max(signedAbs(divergenceVelocity[i][j][k]), norm);
	}
	(*DecoLogger::GetSingleton()) << "norm " << norm << "\n";
#ifdef _DEBUG_TIME
	DOUBLE time = t.getCurrentTime();
	(*DecoLogger::GetSingleton()) << "Form matrix and divergence spent " << time << " seconds\n";
#endif
	//Edit by YangZhanxin 2007/08/06 End
}

BOOL NSSolver::residualApproximateZero(DOUBLE ***residual, const vector<Index3>& indices, INT numX, INT numY, INT numZ)
{
#ifdef _CUDA
	BOOL resultCUDA = residualCUDA(d_r, d_indices, static_cast<INT>(indices.size()));
	return resultCUDA;
#else
	//Edit by YangZhanxin 2007/08/07 Start
	static const DOUBLE epsilon = 1e-20;
	BOOL result = TRUE;
	UINT indexSize = static_cast<UINT>(indices.size());
	for (UINT it = 0; it < indexSize; it++)
	{
		INT i = indices[it].m_i;
		INT j = indices[it].m_j;
		INT k = indices[it].m_k;
		if (signedAbs(residual[i][j][k]) >= epsilon)
		{
			result = FALSE;
		}

	}

	return result;
#endif
	//Edit by YangZhanxin 2007/08/07 End
}
void NSSolver::computePreconditioner(GridField* grid, DOUBLE ***ADiag, DOUBLE ***APlusi, DOUBLE ***APlusj, DOUBLE ***APlusk, DOUBLE ***precon, const vector<Index3>& indices)
{
	//Edit by YangZhanxin 2007/08/07 Start
	static const DOUBLE tau = 0.97;
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;

	INT indiceSize = static_cast<INT>(indices.size());

	for (INT index = 0; index < indiceSize; index++)
	{
		INT i = indices[index].m_i;
		INT j = indices[index].m_j;
		INT k = indices[index].m_k;

		DOUBLE e = ADiag[i][j][k] - ((i - 1 < 0) ? 0 : APlusi[i - 1][j][k] * precon[i - 1][j][k] * APlusi[i - 1][j][k] * precon[i - 1][j][k])
			- ((j - 1 < 0) ? 0 : APlusj[i][j - 1][k] * precon[i][j - 1][k] * APlusj[i][j - 1][k] * precon[i][j - 1][k])
			- ((k - 1 < 0) ? 0 : APlusk[i][j][k - 1] * precon[i][j][k - 1] * APlusk[i][j][k - 1] * precon[i][j][k - 1])
			- tau * (((i - 1 < 0) ? 0 : APlusi[i - 1][j][k] * (APlusj[i - 1][j][k] + APlusk[i - 1][j][k]) * precon[i - 1][j][k] * precon[i - 1][j][k])
			+((j - 1 < 0) ? 0 : APlusj[i][j - 1][k] * (APlusi[i][j - 1][k] + APlusk[i][j - 1][k]) * precon[i][j - 1][k] * precon[i][j - 1][k])
			+((k - 1 < 0) ? 0 : APlusk[i][j][k - 1] * (APlusi[i][j][k - 1] + APlusj[i][j][k - 1]) * precon[i][j][k - 1] * precon[i][j][k - 1]));
		precon[i][j][k] = sqrt(static_cast<DOUBLE>(1 / (e + 1e-30)));
	}

	//Edit by YangZhanxin 2007/08/07 End
}



void NSSolver::applyPreconditioner(GridField* grid, DOUBLE ***ADiag, DOUBLE ***APlusi, DOUBLE ***APlusj, DOUBLE ***APlusk, DOUBLE ***precon, DOUBLE*** vec, DOUBLE ***q, const vector<Index3>& indices, DOUBLE ***result)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;

	Zero3DArray<DOUBLE>(q, numX, numY, numZ);

	size_t indexSize = indices.size();
	for ( size_t index = 0; index < indexSize; index++ )
	{
		INT i = indices[index].m_i;
		INT j = indices[index].m_j;
		INT k = indices[index].m_k;
		DOUBLE t = vec[i][j][k] - ((i - 1 < 0) ? 0 : APlusi[i - 1][j][k] * precon[i - 1][j][k] * q[i - 1][j][k])
			- ((j - 1 < 0) ? 0 : APlusj[i][j - 1][k] * precon[i][j - 1][k] * q[i][j - 1][k])
			- ((k - 1 < 0) ? 0 : APlusk[i][j][k - 1] * precon[i][j][k - 1] * q[i][j][k - 1]);
		q[i][j][k] = t * precon[i][j][k];

	}

	for (INT index = static_cast<INT>(indexSize - 1); index >= 0; index--)
	{
		INT i = indices[index].m_i;
		INT j = indices[index].m_j;
		INT k = indices[index].m_k;
		DOUBLE t = q[i][j][k] - ((i + 1 >= numX) ? 0 : APlusi[i][j][k] * precon[i][j][k] * result[i + 1][j][k])
			- ((j + 1 >= numY) ? 0 : APlusj[i][j][k] * precon[i][j][k] * result[i][j + 1][k])
			- ((k + 1 >= numZ) ? 0 : APlusk[i][j][k] * precon[i][j][k] * result[i][j][k + 1]);

		result[i][j][k] = t * precon[i][j][k];
	}

}
void NSSolver::applyMatrixA(GridField* grid, DOUBLE ***ADiag, DOUBLE ***APlusi, DOUBLE ***APlusj, DOUBLE ***APlusk, DOUBLE*** vec, const vector<Index3>& indices, DOUBLE ***result)
{
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;

	UINT indexSize = static_cast<UINT>(indices.size());
	for ( UINT it = 0; it < indexSize; it++) 
	{
		INT i = indices[it].m_i;
		INT j = indices[it].m_j;
		INT k = indices[it].m_k;
		result[i][j][k] = ADiag[i][j][k] * vec[i][j][k] + APlusi[i][j][k] * ((i + 1 >= numX) ? 0 : vec[i + 1][j][k])
														   + APlusj[i][j][k] * ((j + 1 >= numY) ? 0 : vec[i][j + 1][k])
														   + APlusk[i][j][k] * ((k + 1 >= numZ) ? 0 : vec[i][j][k + 1])
														   + ((i - 1 < 0) ? 0 : APlusi[i - 1][j][k] * vec[i - 1][j][k])
														   + ((j - 1 < 0) ? 0 : APlusj[i][j - 1][k] * vec[i][j - 1][k])
														   + ((k - 1 < 0) ? 0 : APlusk[i][j][k - 1] * vec[i][j][k - 1]);

	}	
}
DOUBLE NSSolver::dotProduct(DOUBLE*** v1, DOUBLE*** v2, const vector<Index3>& indices)
{
	DOUBLE result = 0;

	UINT indexSize = static_cast<UINT>(indices.size());
	for ( UINT it = 0; it < indexSize; it++ )
	{
		INT i = indices[it].m_i;
		INT j = indices[it].m_j;
		INT k = indices[it].m_k;

		result += v1[i][j][k] * v2[i][j][k];

	}
	return result;
}


void checkMatrix(DOUBLE ***ADiag, DOUBLE ***APlusi, DOUBLE ***APlusj, DOUBLE ***APlusk,const vector<Index3>& indices)
{
	INT numIndices = static_cast<INT>(indices.size());

	for (INT it = 0; it < numIndices; it++)
	{
		INT i = indices[it].m_i;
		INT j = indices[it].m_j;
		INT k = indices[it].m_k;
		if (ADiag[i][j][k] <= 0)
			*((INT*)0) = 0;
		if (APlusi[i][j][k] > 0)
			*((INT*)0) = 0;
		if (APlusj[i][j][k] > 0)
			*((INT*)0) = 0;
		if (APlusk[i][j][k] > 0)
			*((INT*)0) = 0;
		DOUBLE total = signedAbs(APlusi[i][j][k]) + signedAbs(APlusj[i][j][k]) + signedAbs(APlusk[i][j][k]);
		if (i - 1 >= 0)
			total += signedAbs(APlusi[i - 1][j][k]);
		if (j - 1 >= 0)
			total += signedAbs(APlusj[i][j - 1][k]);
		if (k - 1 >= 0)
			total += signedAbs(APlusk[i][j][k - 1]);
		if (ADiag[i][j][k] < total)
			*((INT*)0) = 0;
	}
}

void NSSolver::solveForPressure(GridField* grid, DOUBLE ***ADiag, DOUBLE ***APlusi, DOUBLE ***APlusj, DOUBLE ***APlusk, DOUBLE ***divergenceVelocity, const vector<Index3>& indices, BOOL*** domain)
{
	//Edit by YangZhanxin 2007/08/07 Start
	UINT numX = grid->numXElement;
	UINT numY = grid->numYElement;
	UINT numZ = grid->numZElement;
	//checkMatrix(ADiag, APlusi, APlusj, APlusk, indices);
	INT indicesSize = static_cast<INT>(indices.size());


	DOUBLE ***p = grid->p;
	Zero3DArray(p, numX, numY, numZ);
	Zero3DArray(s, numX, numY, numZ);
	Zero3DArray(z, numX, numY, numZ);
	Zero3DArray(precon, numX, numY, numZ);
	Zero3DArray(r, numX, numY, numZ);
	Zero3DArray(q, numX, numY, numZ);
	computePreconditioner(grid, ADiag, APlusi, APlusj, APlusk, precon, indices);
	CopyTo3DArray<DOUBLE>(r, divergenceVelocity, numX, numY, numZ);




	// Conjugate Gradient without Preconditioning
	//if (!residualApproximateZero(r, indices))
	//{
	//	CopyTo3DArray(s, r, numX, numY, numZ);
	//	sigma = dotProduct(r, r, indices);
	//	UINT iterationCount = 0;
	//	static const UINT maxInterationCount = 5000;
	//	for (iterationCount = 0; iterationCount < maxInterationCount; iterationCount++)
	//	{
	//		applyMatrixA(grid, ADiag, APlusi, APlusj, APlusk, s, indices, z);
	//		DOUBLE alpha = sigma / dotProduct(z, s, indices);
	//		for (UINT i = 0; i < numX; i++)
	//		{
	//			for (UINT j = 0; j < numY; j++)
	//			{
	//				for (UINT k = 0; k < numZ; k++)
	//				{
	//					p[i][j][k] += alpha * s[i][j][k];
	//					r[i][j][k] -= alpha * z[i][j][k];
	//				}
	//			}
	//		}

	//		if (residualApproximateZero(r, indices))
	//		{
	//			break;
	//		}
	//		DOUBLE sigmaNew = dotProduct(r, r, indices);
	//		DOUBLE beta = sigmaNew / sigma;
	//		for (UINT i = 0; i < numX; i++)
	//		{
	//			for (UINT j = 0; j < numY; j++)
	//			{
	//				for (UINT k = 0; k < numZ; k++)
	//				{
	//					s[i][j][k] = r[i][j][k] + beta * s[i][j][k];
	//				}
	//			}
	//		}
	//		sigma = sigmaNew;
	//	}	

	
	UINT iterationCount = 0;
	static const UINT maxInterationCount = 500;
	DOUBLE sigma = 1;
	DOUBLE alpha = 0;
#ifdef _DEBUG_TIME
	DecoTimer timer1;
	timer1.startTimer();		

#endif 
	FLOAT timeTotal = 0;
	INT arraySize = (numX + 2) * (numY + 2) * (numZ + 2);
	for (iterationCount = 0; iterationCount < maxInterationCount; iterationCount++)
	{


		applyPreconditioner(grid, ADiag, APlusi, APlusj, APlusk, precon, r, q, indices, z);
		DOUBLE sigmaNew = dotProduct(z, r, indices);
		DOUBLE beta = sigmaNew / sigma;
		INT i, j, k;
		for (INT it = 0; it < indicesSize; it++)
		{
			i = indices[it].m_i;
			j = indices[it].m_j;
			k = indices[it].m_k;
			s[i][j][k] = z[i][j][k] + beta * s[i][j][k];	
		}
		sigma = sigmaNew;
		applyMatrixA(grid, ADiag, APlusi, APlusj, APlusk, s, indices, z);
		DOUBLE tmpDot = dotProduct(z, s, indices);

		DOUBLE alpha = tmpDot ? (sigma / tmpDot) : 0; 
		for (INT it = 0; it < indicesSize; it++)
		{
			i = indices[it].m_i;
			j = indices[it].m_j;
			k = indices[it].m_k;
			p[i][j][k] += alpha * s[i][j][k];
			r[i][j][k] -= alpha * z[i][j][k];
		}

		if (residualApproximateZero(r, indices, numX, numY, numZ))
		{
			break;
		}

	}	
	if (iterationCount >= maxInterationCount)
	{
		(*DecoLogger::GetSingleton()) << "Solve falied\n";
		assert(0);
	}	
#ifdef _DEBUG_TIME

	DOUBLE spentTime = timer1.getCurrentTime();
	(*DecoLogger::GetSingleton()) << (INT)iterationCount << " iterations spent on solving " << (INT)indices.size() << " variables\n";
	(*DecoLogger::GetSingleton()) << "Solving iteration spent " << spentTime << " seconds in total\n";
#endif




}

DOUBLE NSSolver::getPressure(GridField* grid, UINT i, UINT j, UINT k)
{

		return m_grid->p[i][j][k];

}



DOUBLE NSSolver::getU(GridField* grid, UINT i, UINT j, UINT k)
{

		return m_grid->u[i][j][k];
}

DOUBLE NSSolver::getV(GridField* grid, UINT i, UINT j, UINT k)
{
	
		return m_grid->v[i][j][k];

}

DOUBLE NSSolver::getW(GridField* grid, UINT i, UINT j, UINT k)
{

		return m_grid->w[i][j][k];

}

void testFunc(INT i, INT j, INT k, INT checkI, INT checkJ, INT checkK, char dim, DOUBLE value, INT lineNum)
{
	switch (dim)
	{
	case 'u':
		if ((i == checkI || i == checkI + 1) && j == checkJ && k == checkK)
			(*DecoLogger::GetSingleton()) << "update U(" << i << ", " << j << ", " << k << ") to " << value << "at Line " << lineNum << "\n";
		break;
	case 'v':
		if (i == checkI && (j == checkJ || j == checkJ + 1) && k == checkK)
			(*DecoLogger::GetSingleton()) << "update V(" << i << ", " << j << ", " << k << ") to " << value << "at Line " << lineNum << "\n";
		break;

	case 'w':
		if (i == checkI && j == checkJ && (k == checkK || k == checkK + 1))
			(*DecoLogger::GetSingleton()) << "update W(" << i << ", " << j << ", " << k << ") to " << value << "at Line " << lineNum << "\n";
		break;

	}

}
void NSSolver::projectVelocity(GridField* grid, const vector<Index3>& indices, BOOL*** domain)
{
	//Edit by YangZhanxin 2007/08/07 Start
	DOUBLE TIME_STEP = m_scene->GetTimeStep();
#ifdef _DEBUG_TIME
	DecoTimer t;
	t.startTimer();
#endif
	INT numX = grid->numXElement;
	INT numY = grid->numYElement;
	INT numZ = grid->numZElement;
	DOUBLE stepSize = grid->stepSize;
	Zero3DArray(tmpU, numX + 1, numY, numZ);
	Zero3DArray(tmpV, numX, numY + 1, numZ);
	Zero3DArray(tmpW, numX, numY, numZ + 1);
	vector<Index3> indicesU;
	vector<Index3> indicesV;
	vector<Index3> indicesW;

	INT indiceUSize = static_cast<INT>(grid->m_validU.size());
	INT indiceVSize = static_cast<INT>(grid->m_validV.size());
	INT indiceWSize = static_cast<INT>(grid->m_validW.size());
	INT indicesSize = static_cast<INT>(indices.size());
	INT bAllSolidBoundary = 0;
	DecoConfig::GetSingleton()->GetInt("Solver", "bAllSolidBoundary", bAllSolidBoundary);
	for (INT it = 0; it < indiceUSize; it++)
	{

		INT i = grid->m_validU[it].m_i;
		INT j = grid->m_validU[it].m_j;
		INT k = grid->m_validU[it].m_k;

		if (!domain[i][j][k] && i - 1 >= 0 && !domain[i - 1][j][k] && !bAllSolidBoundary)
		{
			continue;
			(*DecoLogger::GetSingleton()) << "Something wrong in project Velocity u(" << i << "," << j << "," << k << ")\n";
		}
		if (i - 1 >= 0 && i < numX)
		{
			tmpU[i][j][k] = grid->u[i][j][k] - TIME_STEP / RHO_WATER * (getPressure(grid, i, j, k) - getPressure(grid, i - 1, j, k)) / grid->stepSize;
			indicesU.push_back(Index3(i, j, k));
		}
	}
	for (INT it = 0; it < indiceVSize; it++)
	{
		INT i = grid->m_validV[it].m_i;
		INT j = grid->m_validV[it].m_j;
		INT k = grid->m_validV[it].m_k;

		if (!domain[i][j][k] && j - 1 >= 0 && !domain[i][j - 1][k] && !bAllSolidBoundary)
		{
			continue;
			(*DecoLogger::GetSingleton()) << "Something wrong in project Velocity v(" << i << "," << j << "," << k << ")\n";
		}
		if (j - 1 >= 0 && j < numY)
		{
			tmpV[i][j][k] = grid->v[i][j][k] - TIME_STEP / RHO_WATER * (getPressure(grid, i, j, k) - getPressure(grid, i, j - 1, k)) / grid->stepSize;
			indicesV.push_back(Index3(i, j, k));
		}
	}
	for (INT it = 0; it < indiceWSize; it++)
	{
		INT i = grid->m_validW[it].m_i;
		INT j = grid->m_validW[it].m_j;
		INT k = grid->m_validW[it].m_k;
		if (!domain[i][j][k] && k - 1 >= 0 && !domain[i][j][k - 1] && !bAllSolidBoundary)
		{
			continue;
			(*DecoLogger::GetSingleton()) << "Something wrong in project Velocity w(" << i << "," << j << "," << k << ")\n";
		}
		if (k - 1 >= 0 && k < numZ)
		{
			tmpW[i][j][k] = grid->w[i][j][k] - TIME_STEP / RHO_WATER * (getPressure(grid, i, j, k) - getPressure(grid, i, j, k - 1)) / grid->stepSize;
			indicesW.push_back(Index3(i, j, k));
		}
	}

	for (INT it = 0; it < indicesSize; it++)
	{
		INT i = indices[it].m_i;
		INT j = indices[it].m_j;
		INT k = indices[it].m_k;

		if (domain[i][j][k])
		{		

#ifdef _SLC

			if (bUseSurfacePressure && grid->isSurfaceCell(i, j, k))
			{
				DOUBLE neibourDist = 0;
				DOUBLE signedDist = grid->phi[i][j][k];//SLC::GetSingleton()->SamplePhi(grid->GetGridCenter(i, j, k));
				if (!grid->isSolidCell(i + 1, j, k) && !grid->isSprayerCell(i + 1, j, k) && grid->type[i + 1][j][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i + 1][j][k]) > 0)
				{
					DOUBLE theta = grid->pTheta[i][j][k].x; 

					tmpU[i + 1][j][k] = grid->u[i + 1][j][k] - TIME_STEP / RHO_WATER * ((theta - 1) / theta * getPressure(grid, i, j, k) - getPressure(grid, i, j, k)) / grid->stepSize;
					indicesU.push_back(Index3(i + 1, j, k));
				}
				if (!grid->isSolidCell(i, j + 1, k) && !grid->isSprayerCell(i, j + 1, k) && grid->type[i][j + 1][k] != GT_FLuid && signedDist <=0 && (neibourDist = grid->phi[i][j + 1][k]) > 0)
				{
					DOUBLE theta = grid->pTheta[i][j][k].y;

					tmpV[i][j + 1][k] = grid->v[i][j + 1][k] - TIME_STEP / RHO_WATER * ((theta - 1) / theta * getPressure(grid, i, j, k) - getPressure(grid, i, j, k)) / grid->stepSize;
					indicesV.push_back(Index3(i, j + 1, k));
				}
				if (!grid->isSolidCell(i, j, k + 1) && !grid->isSprayerCell(i, j, k + 1) && grid->type[i][j][k + 1] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j][k + 1]) > 0)
				{
					DOUBLE theta = grid->pTheta[i][j][k].z; 

					tmpW[i][j][k + 1] = grid->w[i][j][k + 1] - TIME_STEP / RHO_WATER * ((theta - 1) / theta * getPressure(grid, i, j, k) - getPressure(grid, i, j, k)) / grid->stepSize;
					indicesW.push_back(Index3(i, j, k + 1));
				}
				if (!grid->isSolidCell(i - 1, j, k) && !grid->isSprayerCell(i - 1, j, k) && grid->type[i - 1][j][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i - 1][j][k]) > 0)
				{
					DOUBLE theta = grid->mTheta[i][j][k].x;

					tmpU[i][j][k] = grid->u[i][j][k] - TIME_STEP / RHO_WATER * (getPressure(grid, i, j, k) - (theta - 1) / theta * getPressure(grid, i, j, k)) / grid->stepSize;
					indicesU.push_back(Index3(i, j, k));
				}
				if (!grid->isSolidCell(i, j - 1, k) && !grid->isSprayerCell(i, j - 1, k) && grid->type[i][j - 1][k] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j - 1][k]) > 0)
				{
					DOUBLE theta = grid->mTheta[i][j][k].y;
					tmpV[i][j][k] = grid->v[i][j][k] - TIME_STEP / RHO_WATER * (getPressure(grid, i, j, k) - (theta - 1) / theta * getPressure(grid, i, j, k)) / grid->stepSize;
				
					indicesV.push_back(Index3(i, j, k));
				}
				if (!grid->isSolidCell(i, j, k - 1) && !grid->isSprayerCell(i, j, k - 1) && grid->type[i][j][k - 1] != GT_FLuid && signedDist <= 0 && (neibourDist = grid->phi[i][j][k - 1]) > 0)
				{
					DOUBLE theta = grid->mTheta[i][j][k].z;

					tmpW[i][j][k] = grid->w[i][j][k] - TIME_STEP / RHO_WATER * (getPressure(grid, i, j, k) - (theta - 1) / theta * getPressure(grid, i, j, k)) / grid->stepSize;
					indicesW.push_back(Index3(i, j, k));
				}
			}
#endif
		}
	}	


	INT indicesUSize = static_cast<INT>(indicesU.size());
	INT indicesVSize = static_cast<INT>(indicesV.size());
	INT indicesWSize = static_cast<INT>(indicesW.size());

	for (INT index = 0; index < indicesUSize; index++)
	{
		INT i = indicesU[index].m_i;
		INT j = indicesU[index].m_j;
		INT k = indicesU[index].m_k;
		if (i > numX || i < 0 || j >= numY || j < 0 || k >= numZ || k < 0)
		{
			(*DecoLogger::GetSingleton()) << "index out of range in project velocity\n";
			*((INT*)0) = 0;
		}
		grid->u[i][j][k] = tmpU[i][j][k];
	}
	for (INT index = 0; index < indicesVSize; index++)
	{
		INT i = indicesV[index].m_i;
		INT j = indicesV[index].m_j;
		INT k = indicesV[index].m_k;
		if (i >= numX || i < 0 || j > numY || j < 0 || k >= numZ || k < 0)
		{
			(*DecoLogger::GetSingleton()) << "index out of range in project velocity\n";
			*((INT*)0) = 0;
		}
		grid->v[i][j][k] = tmpV[i][j][k];
		//if (abs(grid->v[i][j][k]) > 1e-6)
		//	(*DecoLogger::GetSingleton()) << "v(" << i << "," << j << "," << k << ") is " << grid->v[i][j][k] << "\n";
	}
	for (INT index = 0; index < indicesWSize; index++)
	{
		INT i = indicesW[index].m_i;
		INT j = indicesW[index].m_j;
		INT k = indicesW[index].m_k;
		if (i >= numX || i < 0 || j >= numY || j < 0 || k > numZ || k < 0)
		{
			(*DecoLogger::GetSingleton()) << "index out of range in project velocity\n";
			*((INT*)0) = 0;
		}
		grid->w[i][j][k] = tmpW[i][j][k];
	}

#ifdef _DEBUG_TIME
	DOUBLE time = t.getCurrentTime();
	(*DecoLogger::GetSingleton()) << "Project Velocity spent " << time << " seconds\n";
#endif
	//Edit by YangZhanxin 2007/08/08 End
}
vector3 NSSolver::GetVelocity(const vector3& pos, BOOL fromCoarse)
{

	return m_grid->SampleVelocity(pos);
}

void NSSolver::ToggleShowGridLayer()
{
	m_showLayer = 1 - m_showLayer;
}

void NSSolver::Render (DecoRenderInterface* RI, DecoLight** Lights, UINT numEffectiveLights)
{
	if (m_showLayer)
		m_grid->Render(RI, Lights, numEffectiveLights);
}

void NSSolver::SetScene(DecoScene* scene)
{
	m_scene = scene;
}

void NSSolver::SetGridField(GridField* gridField)
{
	assert(gridField);
	m_grid = gridField;

	allocateAllArrays(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement);
	resultField.SetNumElement(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement, FALSE);
	heap.setSize(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement);
	resultField2.SetNumElement(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement);
	errorField.SetNumElement(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement);
	tmpMinusVelField.SetNumElement(m_grid->numXElement, m_grid->numYElement, m_grid->numZElement);

	INT bAccurateCoupling = 0;
	DecoConfig::GetSingleton()->GetInt("Solver", "bAccurateCoupling", bAccurateCoupling);
	if (bAccurateCoupling)
	{
		if (mCoupler)
			delete mCoupler;
		mCoupler = new TwowayCoupler(gridField);
	}
}

void NSSolver::destroyAllArrays()
{

	if (!m_grid)
		return;
	INT numX = m_grid->numXElement;
	INT numY = m_grid->numYElement;
	INT numZ = m_grid->numZElement;

	if (divergenceVelocity)
	{
		Free3DArray(divergenceVelocity, numX, numY, numZ);
		divergenceVelocity = NULL;
	}
	if (ADiag)
	{
		Free3DArray(ADiag, numX, numY, numZ);
		ADiag = NULL;
	}
	if (APlusi)
	{
		Free3DArray(APlusi, numX, numY, numZ);
		APlusi = NULL;
	}
	if (APlusj)
	{
		Free3DArray(APlusj, numX, numY, numZ);
		APlusj = NULL;
	}
	if (APlusk)
	{
		Free3DArray(APlusk, numX, numY, numZ);
		APlusk = NULL;
	}
	if (tmpDomain)
	{
		Free3DArray(tmpDomain, numX + 1, numY + 1, numZ + 1);
		tmpDomain = NULL;
	}
	//if (p)
	//{
	//	Free3DArray(p, numX, numY, numZ);
	//	p = NULL;
	//}
	if (s)
	{
		Free3DArray(s, numX, numY, numZ);
		s = NULL;
	}
	if (z)
	{
		Free3DArray(z, numX, numY, numZ);
		z = NULL;
	}
	if (precon)
	{
		Free3DArray(precon, numX, numY, numZ);
		precon = NULL;
	}
	if (r)
	{
		Free3DArray(r, numX, numY, numZ);
		r = NULL;
	}
	if (q)
	{
		Free3DArray(q, numX, numY, numZ);
		q = NULL;
	}
	if (tmpU)
	{
		Free3DArray(tmpU, numX + 1, numY, numZ);
		tmpU = NULL;
	}
	if (tmpV)
	{
		Free3DArray(tmpV, numX, numY + 1, numZ);
		tmpV = NULL;
	}
	if (tmpW)
	{
		Free3DArray(tmpW, numX, numY, numZ + 1);
		tmpW = NULL;
	}

}

void NSSolver::allocateAllArrays(INT numX, INT numY, INT numZ)
{
	destroyAllArrays();
	divergenceVelocity = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	ADiag = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	APlusi = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	APlusj = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	APlusk = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	tmpDomain = Malloc3DArray<BOOL>(numX + 1, numY + 1, numZ + 1);
	//p = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	s = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	z = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	precon = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	r = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	q = Malloc3DArray<DOUBLE>(numX, numY, numZ);
	tmpU = Malloc3DArray<DOUBLE>(numX + 1, numY, numZ);
	tmpV = Malloc3DArray<DOUBLE>(numX, numY + 1, numZ);
	tmpW = Malloc3DArray<DOUBLE>(numX, numY, numZ + 1);


}


void NSSolver::calculateVorticity()
{
	calculateVorticity(m_grid);
}

void NSSolver::calculateVorticity(GridField* grid)
{
	grid->CalculateVorticity();
}


void NSSolver::checkIncompressible(GridField* grid)
{
	INT compDomainSize = static_cast<INT>(grid->m_compDomain.size());
	INT bUpdateGrid = 0;
	DecoConfig::GetSingleton()->GetInt("Solver", "bUpdateGridType", bUpdateGrid);
	if (grid == m_grid)
		{
		(*DecoLogger::GetSingleton()) << "Check Incompressibility in coarse grid\n";
		}
		else
		{
		(*DecoLogger::GetSingleton()) << "Check Incompressibility in detailed grid\n";
	}

	for (INT it = 0; it < compDomainSize; it++)
	{
		INT i = grid->m_compDomain[it].m_i;
		INT j = grid->m_compDomain[it].m_j;
		INT k = grid->m_compDomain[it].m_k;
		if (!m_grid->isFluidCell(i, j, k))
			continue;

		DOUBLE divergence = (grid->u[i + 1][j][k] - grid->u[i][j][k] + grid->v[i][j + 1][k] - grid->v[i][j][k] + grid->w[i][j][k + 1] - grid->w[i][j][k]) / grid->stepSize;

		if (signedAbs(divergence) >= 1e-4)
		{
			(*DecoLogger::GetSingleton()) << "Warning! Divergence of grids (" << i << "," << j << "," << k <<") is " << divergence << "\n";
			(*DecoLogger::GetSingleton()) << "Formed Divergence is " << divergenceVelocity[i][j][k] << "\n";
			
			if (bUpdateGrid)
			{	
			(*DecoLogger::GetSingleton()) << "Adiag = " << ADiag[i][j][k] << "\n";
			(*DecoLogger::GetSingleton()) << "Aplusi = " << APlusi[i][j][k] << "\n";
			if (i - 1 >= 0)
				(*DecoLogger::GetSingleton()) << "Aminusi = " << APlusi[i - 1][j][k] << "\n";
			(*DecoLogger::GetSingleton()) << "Aplusj = " << APlusj[i][j][k] << "\n";
			if (j - 1 >= 0)
				(*DecoLogger::GetSingleton()) << "Aminusj = " << APlusj[i][j - 1][k] << "\n";
			(*DecoLogger::GetSingleton()) << "Aplusk = " << APlusk[i][j][k] << "\n";
			if (k - 1 >= 0)
				(*DecoLogger::GetSingleton()) << "Aminusk = " << APlusk[i][j][k - 1] << "\n";

			(*DecoLogger::GetSingleton()) << "Typeplusi = " << grid->type[i + 1][j][k] << "\n";
			if (i - 1 >= 0)
				(*DecoLogger::GetSingleton()) << "Typeminusi = " << grid->type[i - 1][j][k] << "\n";
			(*DecoLogger::GetSingleton()) << "Typeplusj = " << grid->type[i][j + 1][k] << "\n";
			if (j - 1 >= 0)
				(*DecoLogger::GetSingleton()) << "Typeminusj = " << grid->type[i][j - 1][k] << "\n";
			(*DecoLogger::GetSingleton()) << "Typeplusk = " << grid->type[i][j][k + 1] << "\n";
			if (k - 1 >= 0)
				(*DecoLogger::GetSingleton()) << "Typeminusk = " << grid->type[i][j][k - 1] << "\n";

			(*DecoLogger::GetSingleton()) << "Pressure  (" << i << "," << j << "," << k <<") is " << getPressure(grid, i, j, k) << "\n";
			(*DecoLogger::GetSingleton()) << "Pressure  (" << i + 1 << "," << j << "," << k <<") is " << getPressure(grid, i + 1, j, k) << "\n";
			(*DecoLogger::GetSingleton()) << "Pressure  (" << i - 1 << "," << j << "," << k <<") is " << getPressure(grid, i - 1, j, k) << "\n";
			(*DecoLogger::GetSingleton()) << "Pressure  (" << i << "," << j + 1 << "," << k <<") is " << getPressure(grid, i, j + 1, k) << "\n";
			(*DecoLogger::GetSingleton()) << "Pressure  (" << i << "," << j - 1 << "," << k <<") is " << getPressure(grid, i, j - 1, k) << "\n";
			(*DecoLogger::GetSingleton()) << "Pressure  (" << i << "," << j << "," << k + 1 <<") is " << getPressure(grid, i, j, k + 1) << "\n";
			(*DecoLogger::GetSingleton()) << "Pressure  (" << i << "," << j << "," << k - 1 <<") is " << getPressure(grid, i, j, k - 1) << "\n";

			*((INT*)0) = 0;
		}
	}
	}

}


void NSSolver::Serialize(DecoArchive& Ar)
{
#ifdef _SLC
	Ar << bRemoveBottom;
#endif
	Ar << detailDepth;
	Ar << marchingGrids;
	Ar << m_viscocity;
	Ar << m_tension;
	Ar << m_force;
	Ar << m_blockSizeX;
	Ar << m_blockSizeY;
	Ar << m_blockSizeZ;
	INT sizeUpdatingGrids = static_cast<INT>(m_blockToUpdate.size());
	Ar << sizeUpdatingGrids;
	for (list<Index3>::const_iterator it = m_blockToUpdate.begin(); it != m_blockToUpdate.end(); ++it)
	{
		Ar << (*it);
	}

}
void NSSolver::Deserialize(DecoArchive& Ar)
{
#ifdef _SLC
	Ar >> bRemoveBottom;
#endif
	Ar >> detailDepth;
	Ar >> marchingGrids;
	Ar >> m_viscocity;
	Ar >> m_tension;
	Ar >> m_force;
	Ar >> m_blockSizeX;
	Ar >> m_blockSizeY;
	Ar >> m_blockSizeZ;
	INT sizeUpdatingGrids = 0;
	Ar >> sizeUpdatingGrids;
	for (INT i = 0; i < sizeUpdatingGrids; i++)
	{
		Index3 idx;
		Ar >> idx;
		m_blockToUpdate.push_back(idx);
	}
}

void NSSolver::UpdateFluidGridType(bool bRefillParticles)
{
	INT bUpdateGrid = 1, airMargin = 1;
	DecoConfig::GetSingleton()->GetInt("Solver", "bUpdateGridType", bUpdateGrid);
	DecoConfig::GetSingleton()->GetInt("Solver", "airMargin", airMargin);

	if (bUpdateGrid)
	{
		updateGridType();
	}
	else
	{
		fillAllGridWithFluid(airMargin, bRefillParticles);
	}
}

CSRMatrix* NSSolver::GetCSRMatrix()
{
	//if (mbMatrixFormed)
	{
		return mCoupler->GetCSRMatrix();
	}
	return NULL;
}

FLuidSparseMatrix* NSSolver::GetFluidMatrix()
{
	//if (mbMatrixFormed)
	{
		return mCoupler->GetFluidMatrix();
	}
	return NULL;
}

void NSSolver::FormPoissonMatrix()
{
	INT bAccurateCoupling = 0;
	DecoConfig::GetSingleton()->GetInt("Solver", "bAccurateCoupling", bAccurateCoupling);

	if (bAccurateCoupling)
	{
		//mbMatrixFormed = true;

		DOUBLE TIME_STEP = m_scene->GetTimeStep();

		mCoupler->FormMatrix(TIME_STEP, RHO_WATER);
	}
}

void NSSolver::Solve ()
{
	INT bConvection = 1, bAddForce = 1, bProjection = 1, bExtrapolation = 1;
	INT bAccurateCoupling = 0;
	DecoConfig::GetSingleton()->GetInt("Solver", "bAddForce", bAddForce);
	DecoConfig::GetSingleton()->GetInt("Solver", "bConvection", bConvection);
	DecoConfig::GetSingleton()->GetInt("Solver", "bProjection", bProjection);
	DecoConfig::GetSingleton()->GetInt("Solver", "bExtrapolate", bExtrapolation);
	DecoConfig::GetSingleton()->GetInt("Solver", "bAccurateCoupling", bAccurateCoupling);

#ifdef _DEBUG_TIME
	DecoTimer timer;
	timer.startTimer();
	DOUBLE totalTime = 0;
#endif


#ifdef _DEBUG_TIME
	DOUBLE currentTime = timer.getCurrentTime();
	*(DecoLogger::GetSingleton()) << "UpdateParticles spent " << (currentTime) << " seconds\n";
	timer.resetTimer();
	totalTime += currentTime;
#endif	

	if (bConvection)
		convection();

#ifdef _DEBUG_TIME
	currentTime = timer.getCurrentTime();
	*(DecoLogger::GetSingleton()) << "Convection spent " << (currentTime)  << " seconds\n";
	timer.resetTimer();
	totalTime += currentTime;
#endif	
#ifndef _CUDA
	if (bAddForce)
		addForce();
#endif
#ifdef _DEBUG_TIME
	currentTime = timer.getCurrentTime();
	*(DecoLogger::GetSingleton()) << "AddForce spent " << (currentTime) << " seconds\n";
	timer.resetTimer();
	totalTime += currentTime;
#endif
	if (bProjection)
	{
		if (bAccurateCoupling)
		{
			DOUBLE TIME_STEP = m_scene->GetTimeStep();

			mCoupler->Projection(TIME_STEP, RHO_WATER, !mbMatrixFormed);

		}
		else
		{
			*(DecoLogger::GetSingleton()) << "Wrong! Wrong! Wrong \n";

			projection();

		}
		//checkIncompressible(m_grid);

	}

#ifdef _DEBUG_TIME
	currentTime = timer.getCurrentTime();
	*(DecoLogger::GetSingleton()) << "Projection spent " << (currentTime) << " seconds\n";
	timer.resetTimer();
	totalTime += currentTime;
#endif
	//testBoundaryVelocity();

	if (bExtrapolation)
		extrapolateVelocity();
	enforceSolidVeloctiy();

#ifdef _DEBUG_TIME
	currentTime = timer.getCurrentTime();
	*(DecoLogger::GetSingleton()) << "Extrapolation spent " << (currentTime) << " seconds\n";
	timer.resetTimer();
	totalTime += currentTime;
#endif

	calculateVorticity();
	*(DecoLogger::GetSingleton()) << "NavierStokes Solver took " << (totalTime) << " seconds in total\n\n";

	mbMatrixFormed = false;
}